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ABSTRACT 


a  new  type  of  gait  and  steering  algorithm  for  use  by  a  six-legged  walking 
machine  is  developed  and  presented  in  this  study.  The  spatially  oriented  tripod 
follow-the-leader  gait  is  an  extension  of  previous  studies  of  temporal  follow-the- 
leader  gaits,  and  should  prove  useful  for  all-terrain  walking  vehicles,  such  as  the 
Adaptive  Suspension  Vehicle.  Tractor-trailer  style  steering  is  introduced  as  an 


effort  to  tailor  steering  control  for  this  type  of  gait.  Both  gait  and  steering 
algorithms  are  implemented  on  a  color  graphics  computer  simulation  for  study 
and  comparison  with  other  walking  algorithms.  - 
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I.  INTRODUCTION 

It  is  estimated  that  nearly  half  of  the  Earth's  land  surface  is  inaccessible  to 
wheeled  and  tracked  vehicles  [Ref.  l].  Yet  almost  all  of  this  same  area  can  be 
successfully  traversed  by  animals  and  man.  This  great  difference  in  mobility  has 
motivated  research  into  the  creation  of  a  practical  legged  vehicle  or 
walking  machine. 

The  advantages  of  legged  locomotion  can  largely  be  attributed  to  the 
flexibility  offered  in  leg  placement  and  support.  Wheeled  vehicles,  and  to  a  lesser 
extent  tracked  vehicles,  are  confined  to  a  more  or  less  continuous,  relatively  flat 
and  obstruction  free  paths  along  the  ground.  The  leg’s  flexibility  allows  the 
utilization  of  discontinuous  support  regions  on  the  ground  and  the  adaptation  to 
terrain  slope.  A  legged  vehicle  may  potentially  use  obstructions  for  support  as  it 
climbs  over  those  obstacles  which  it  decides  to  not  simply  ignore. 

A  second  advantage  of  legs  involves  the  means  of  obtaining  traction  in  soft 
soil.  A  wheel  or  track  creates  a  depression  or  rut  from  which  it  must  continually 
work  to  climb  out.  Slippage  causes  the  wheel  or  track  spin,  possibly  digging  a 
deeper  hole.  A  leg,  however,  may  be  lifted  vertically  out  of  its  depression, 
minimizing  the  work  required.  In  addition,  any  back  slip  caused  by  the  vehicle 
stepping  pushes  up  soil  behind  the  foot  and  improves  traction.  [Ref.  2] 


The  combination  of  flexible  coordination  and  increased  traction  provides  a 
potential  for  greater  speed  and  less  power  consumption  while  operating  over  rough 
and  otherwise  unsuitable  terrain.  Other  advantages  of  legged  locomotion  include 
possible  improved  comfort  in  ride  due  to  the  adaptive  nature  of  legged  support  on 
uneven  terrain,  the  ability  to  test  soil  conditions  prior  to  placement  of  weight  on 
the  legs,  and  the  relatively  small  footprint  left  in  the  soil.  The  latter  may  prove 
especially  important  for  agricultural  work,  where  the  disturbance  of  crops  is  to  be 
minimized,  or  for  military  vehicles  navigating  areas  suspected  of  containing 
landmines. 

A.  GOALS 

The  purpose  of  this  study  is  to  explore  a  new  type  of  gait  and  steering 
algorithm  for  the  use  of  legged  walking  machines.  The  gait  is  a  particular  type  of 
tripod  gait,  which  can  be  considered  as  an  extension  of  the  temporal  follow-the- 
leader  gait  [Ref.  3],  into  the  spatial  domain.  The  steering  algorithm  to  be 
investigated  along  with  this  style  of  gait  borrows  from  the  concept  of  driving  a 
wheeled  tractor-trailer  vehicle.  It  is  believed  that  this  steering  algorithm  may  be 
particularly  well  suited  for  the  fixed  foothold  position  requirements  of  follow-the- 
leader  gaits. 

The  machine  chosen  as  a  physical  reference  for  the  study  is  the  Adaptive 
Suspension  Vehicle  (ASV).  This  is  a  self-contained,  six-legged  vehicle  currently 


being  evaluated  at  the  Ohio  State  University  for  rough-terrain  locomotion.  The 
ASV  is  a  Defense  Advanced  Research  Projects  Agency  (DARPA)  proof  of  concept 
project. 

A  secondary  goal  is  to  develop  a  simulation  model  with  which  to  study 
walking  gaits  and  control  algorithms  in  general  for  the  ASV.  This  model  is 
developed  along  the  general  lines  of  the  simulation  previously  presented  by  Lee 
[Ref.  4],  incorporating  several  of  his  model’s  features,  including  omni-directional 
control,  foot  movement,  and  body  attitude  and  altitude  regulation  algorithms.  In 
addition,  this  simulation  is  to  have  the  features  of  operation  in  either  the  new 
follow-the-leader  tripod  gait  mode  or  in  Lee’s  "forward  wave"  tripod  gait  mode, 
an  enhancement  of  realism  with  a  detailed  color  graphics  display,  and  a  menu 
system  controlled  with  a  single  mouse  button. 

B.  ORGANIZATION 

Chapter  II  provides  a  brief  overview  of  the  previous  work  relating  to  this 
study.  It  includes  a  discussion  of  state  of  the  art  legged  vehicles,  tripod  follow- 
the-leader  gaits,  tripod  gaits,  stability  and  simulation  displays. 

A  detailed  discussion  of  the  ASV’  simulation  problem  is  presented  in  Chapter 
III.  This  chapter  covers  the  configuration  of  the  vehicle,  the  gait  and  steering 
algorithms,  the  simplifications  assumed  in  the  construction  of  the  model,  and  the 
kinematics  involved  in  making  the  ASV’  model  walk.  The  final  section  in  this 


chapter  describes  the  IRIS-2400  simulation  hardware  and  software  on  which  the 
model  was  developed. 

The  simulation  program's  operation  and  functions  are  presented  in  Chapter 
IV.  This  includes  a  complete  description  of  the  operation  of  the  program  controls 
and  display  features.  This  is  followed  by  a  discussion  of  the  means  by  which 
graphics  are  programmed  on  the  IRIS,  and  by  a  description  of  the  organization 
and  flow  of  the  program  and  its  modules. 

Chapter  V  is  a  review  of  the  performance  of  the  simulation.  It  includes  a 
brief  subjective  view  on  the  feel  of  driving  in  the  two  modes. 

The  final  chapter  summarizes  the  contributions  of  this  study.  It  also  contains 
comments  on  possible  directions  for  future  research.  The  program  code  is  listed  in 
the  appendix. 


II.  SURVEY  OF  PREVIOUS  WORK 

A.  INTRODUCTION 

The  last  quarter  of  a  century  has  witnessed  intense  efforts  to  build  machines 
that  walk.  Difficulties  facing  researchers  include  the  problems  of  controlling  the 
many  degrees  of  freedom  necessary  in  a  maneuverable  leg.  maintaining  vehicle 
stability,  creating  energy  efficient  motion,  and  adapting  the  walking  motions  to 
unstructured  terrain.  With  the  advent  of  compact  computer  technology  and 
computer-aided  simulation  and  design,  serious  progress  is  now  being  made  in 
overcoming  these  problems.  [Ref.  5] 

Several  promising  working  designs  have  emerged  in  the  last  ten  years.  Some 
of  the  most  prominent  include  the  Perambulating  Vehicle  II  (PVII)  at  the  Tokyo 
Institute  of  Technology,  the  Carnegie-Mellon  University  hexapod,  the  Odetics  Inc. 
ODEX  I,  and  the  Adaptive  Suspension  Vehicle  (ASV)  developed  at  the  Ohio 
State  University. 

The  PVII  is  a  light-weight  laboratory  model  quadruped,  developed  in  1980. 
It  features  one  of  the  first  pantograph  leg  constructions  designed  specifically  to 
provide  simplified  leg  coordination  and  energy  efficient  walking.  Using  tactile  foot 
sensors  and  a  microcomputer  mounted  near  the  vehicle,  the  PVII  is  able  to  probe 
for  footholds  and  maneuver  over  obstacles.  [Ref.  6] 


The  hexapod  developed  at  the  Carnegie-Mellon  University  in  1982  is  a  self- 
contained  walking  machine  large  enough  to  carry  its  operator.  It  uses  a  gasoline 


engine  to  provide  power  to  the  legs  via  a  set  of  hydraulic  actuators.  The 
movements  of  the  individual  legs  are  controlled  by  a  series  of  passive  hydraulic 
circuits.  A  built-in  microprocessor  interprets  the  driver's  commands  and  specifies 
the  correct  series  of  leg  movement  patterns  to  be  used.  This  arrangement  frees 
the  single  microprocessor  from  the  need  to  compute  each  foot  trajectory.  [Ref.  7] 

The  ODEX  I  is  a  commercial  design  introduced  in  1983  [Ref.  8].  An 
improved  version,  sometimes  referred  to  as  ODEX  II,  is  being  developed  for  near- 
term  use  in  nuclear  power  plants  [Ref.  9].  The  ODEX  series  makes  use  of  a 
unique  circular  arrangement  of  six  planar  pantograph  legs  which  allow  the 
vehicles  to  adjust  their  profile  for  negotiation  of  narrow  passages.  The  ODEX 
walking  machines  are  directed  through  a  radio  or  fiber-optic  link  from  the 
operator  to  an  on-board  supervisory-level  microprocessor.  Each  leg  is  controlled 
by  a  dedicated  lower-level  microprocessor  which  receives  instructions  from  the 
supervisory  level  microprocessor.  The  new  ODEX  hexapod  is  also  being  equipped 
with  a  center-mounted  arm  for  remote  manipulation  of  objects,  such  as  valves,  in 
hazardous  environments.  [Ref.  8] 

The  Adaptive  Suspension  Vehicle  (Figure  2.1),  currently  being  tested  at  Ohio 
State  University,  is  the  first  computer-coordinated  legged  vehicle  designed  and 
built  for  operation  on  natural  terrain  [Ref.  10).  This  hexapod  walking  machine  is 

completely  self-contained,  and  is  capable  of  carrying  the  driver,  a  500  lb.  internal 
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Figure  2.1  Adaptive  Suspension  Vehicle 


payload,  computer  and  control  circuitry,  and  power  system,  in  an  outdoor 
environment.  The  ASV  is  the  vehicle  modeled  in  this  study.  A  more  detailed 
description  of  the  ASV  follows  in  Section  3.B. 

The  remaining  sections  of  this  chapter  concern  gaits  used  by  walking 
machines,  vehicle  steering,  the  walking  machine  stability  problem,  and  graphical 
representation  of  the  vehicle’s  motion.  The  gait  and  stability  sections  are  oriented 
towards  six-legged  vehicles  such  as  the  ASV. 

B.  GAIT  SELECTION 
1.  Definitions 

A  gait  is  a  mode  of  locomotion  for  a  vehicle  or  animal  distinguished  by  a 
specific  pattern  of  lifting  and  placing  of  the  feet.  Gaits  in  general  may  be 
described  using  the  event  sequence  notation  introduced  by  McGhee  and  Jain  [Ref. 
11].  The  integer  i  in  such  a  sequence  corresponds  to  the  event  of  placing  foot  i  on 
the  ground.  The  lifting  of  the  same  foot  is  represented  by  the  integer  i  +  n, 
where  n  equals  the  number  of  legs.  For  the  ASV,  legs  are  numbered  on  the  left 
side  (1.  3,  5)  from  the  front  to  the  rear,  and  on  the  right  side  (2,  4,  6)  in  the  same 
order. 

A  periodic  gait  is  one  that  repeats  the  lifting  and  placing  pattern,  and 
thus  is  represented  by  one  cycle  of  events.  A  periodic  gait  is  said  to  be 
nonsingular  if  no  two  of  its  events  occur  simultaneously.  McGhee  [Ref.  12] 
demonstrated  the  existence  of  39.916.800  possible  nonsingular  periodic  hexapod 


gaits.  The  total  number  of  possible  hexapod  gaits  is  a  much  larger  and  unknown 
number  [Ref.  5j.  This  makes  the  selection  of  an  optimum  gait  a  very  difficult 
problem.  However,  this  thesis  is  concerned  with  a  single  type  of  gait  sequence, 
the  tripod  sequence.  These  are  singular  gaits,  in  that  more  than  one  leg  is  placed 
at  a  given  instant  [Ref.  3j. 

A  periodic  gait  is  considered  symmetrical  when  the  stepping  pattern  on 
one  side  of  the  body  is  identical  to  that  on  the  opposite  side  and  separated  in  time 
by  exactly  one-half  of  the  gait  period  [Ref.  12].  Symmetry  tends  to  simplify  the 
required  leg  coordination  algorithms. 

The  pitch  of  a  gait  is  the  distance  between  footholds,  measured  in  body 
lengths  (defined  as  the  distance  between  the  front  and  rear  leg  reference 
positions).  Leg  stroke  is  the  linear  distance  the  foot  travels  with  respect  to  the 
body  when  occupying  a  particular  foothold.  Leg  stroke  is  also  expressed  in  terms 
of  body  lengths. 

2.  Follow-the- Leader  Gaits 

A  follow -the- leader { FTL)  gait  is  one  in  which  the  middle  and  rear  legs 
on  each  side  of  the  body  step  in  the  foothold  locations  previously  occupied  by  the 
leading  legs  [Ref.  13].  Creeping  FTL  gaits  (in  which  at  most  one  leg  is  in  the  air 
at  any  time  [Ref.  14]).  were  first  studied  by  Ozguner,  Tsai,  and  McGhee  [Ref.  3]. 
Using  a  temporal  framework,  they  narrowed  the  number  of  possible  FTL  creeping 
gaits  to  30.  of  which  they  found  five  to  be  symmetrically  realizable. 


Expanding  to  a  spatial  reference  frame  greatly  increases  the  number  of 
gaits  in  this  category.  A  tripod  creeping  gait  can  be  defined  as  one  in  which  the 
legs  are  placed  in  alternating  groups  of  three,  with  each  group  forming  a  tripod  of 
support.  In  the  case  of  the  ASV,  the  two  possible  tripods  are  the  leg  sets  (14  5) 
and  (2  3  6). 

The  possible  distinct  tripod  creeping  gaits  can  be  ennumerated  using  an 
approach  similar  to  that  in  Ozguner  et  al.  [Ref.  3].  Choosing  the  placement  of  leg 
1  as  a  reference,  evidently  there  are  two  possibilities  for  the  relative  ordering  of 
legs  4  and  5,  and  three  possible  locations  in  each  sequence  for  the  insertion  of  the 
alternate  group  of  legs.  Furthermore,  the  placing  of  the  alternate  leg  group  (2  3 
6)  can  be  accomplished  in  six  distinct  ways.  Table  2.1  lists  the  36  possible 
nonsingular  placing  sequences. 

It  might  first  appear  strange  that  the  sequence  (  1  2  3  6  4  5  )  is  included 
in  Table  2.1.  However  taking  two  periods  together,  the  sequence  becomes 
(123645123645),  which  clearly  shows  that  the  placement  of  the  legs 
occur  in  alternating  groups  of  three. 

Comparing  the  entries  in  Table  2.1  to  those  in  the  table  of  Ozguner  et  al., 
one  can  see  that  none  of  these  sequences  are  listed  in  the  latter  work.  This  is 
because  the  sequences  here  are  not  temporally  follow-the-leader.  Yet  they  all  are 
spatial  FTL  gaits.  This  can  be  seen  from  the  gait  kinematics  of  the  example 


shown  in  Figure  2.2. 


TABLE  2.1. 


Gait 

Number 


PLACING 


Placing 

Sequence 


123645 

126345 

132645 

136245 

162345 

163245 

123654 

26354 

132654 

36254 

162354 

63254 

142365 

142635 

143265 

143625 

146235 

146325 

52364 

152634 

153264 

153624 

156234 

156324 

145236 

145263 

145326 

145362 

145623 

145632 

154236 

154263 

154326 

154362 

154623 

154632 


SEQUENCES  FOR  TRIPOD  CREEPING  FTL  GAITS 


Tripod  2 
Insertion 
Position 


Tripod  1 
Subsequence 


Tripod  2 
Subsequence 
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It  is  possible  to  alternate  body  motion  with  leg  placement  in  the  tripod 


gait  (Figure  2.3).  This  yields  a  pattern  of  movement  that  is  compatible  with  the 
general  notion  of  a  creeping  gait  [Ref.  3).  It  should  be  noted,  however,  that  while 
such  a  strategy  may  improve  the  static  stability  of  the  gait  [Ref.  3],  the 
intermittent  body  motion  increases  the  leg  stroke  by  a  factor  of  two,  which 
greatly  increases  the  required  working  volume  of  the  legs.  For  this  reason,  and 
also  because  intermittent  body  motion  slows  the  average  vehicle  forward  speed, 
only  the  continuous  body  motion  alternative  will  be  considered  further  in  this 
thesis. 

3.  Singular  Tripod  Gaits 

Tripod  gaits  have  proved  to  provide  a  good  compromise  between 
stability,  maneuverability,  and  ease  of  control  for  the  Ohio  State  University 
Hexapod,  the  ODEX  I,  and  the  ASV.  For  this  reason  tripod  gaits  were  chosen  for 
this  simulation  study. 

It  can  be  seen  that  a  tripod  gait  is  actually  a  special  limiting  case  of  a 
creeping  gait,  where  the  time  between  the  placement  of  individual  legs  within  a 
tripod  grouping  approaches  zero.  Of  the  very  large  (unknown)  number  of  gait 
sequences  possible,  only  one  can  be  classified  as  a  singular  tripod  gait  sequence. 
All  differences  among  varieties  of  tripod  gaits  are  therefore  a  function  of 
kinematics  only. 
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All  legs 
supporting 


Move  body 


Move  legs 


Figure  2.3  Sequence  of  Stepping  for  a  Tripod  FTL  with 
Pitch  of  1/3  and  Alternating  Body  and  Leg  Motion 


The  most  frequently  used  tripod  gait  is  the  limiting  form  of  the 
forward  wave  gait ,  where  the  duty  cycle1,  3.  approaches  1/2  [Ref.  4,5).  This 
study  introduces  the  singular  FTL  tripod  gait.  Both  of  these  gaits  are 
implemented  in  the  walking  algorithms  of  this  simulation  and  are  described 
further  in  Chapter  III. 

It  is  interesting  to  note  that  potentially  the  fastest  forward  wave  tripod 
gait  for  the  ASV  is  an  FTL  tripod  gait  with  a  pitch  of  one  (Fig.  2.4).  This  of 
course  can  only  be  considered  a  true  FTL  gait  if  the  feet  are  assumed  to  be 
dimensionless.  In  order  to  prevent  the  legs  from  interfering  with  one  another,  the 
duty  cycle  might  be  made  slightly  less  than  1/2.  This  would  momentarily  leave 
the  vehicle  with  no  supporting  legs  in  contact  with  the  ground.  It  would  also 
have  the  disadvantage  of  not  providing  sufficient  time  for  possible  foothold 
searches  by  the  leading  legs. 

C.  STEERING 

There  are  several  different  approaches  to  steering  currently  used  by  ground 
vehicles.  The  most  familiar  method  is  articulated ,  or  automotive  style  steering 
[Ref.  15).  With  a  steering  wheel,  accelerator  and  brake,  the  driver  of  an 
automobile  can  directly  control  the  vehicle's  turning  radius  and  forward  velocity. 


1  The  duty  cycle  is  the  fraction  of  the  leg  cycle  used  for  supporting  the  body. 
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Tracked  vehicles,  on  the  other  hand,  most  frequently  utilize  skid  steering.  By 
operating  the  sets  of  tracks  at  differing  rates,  the  driver  controls  the  turning  rate 
and  forward  velocity  of  the  vehicle. 

Tractor-trailers  use  still  another  type  of  steering.  Here  the  driver  steers  far 
forward  of  the  vehicle’s  center  of  gravity.  The  trailer  follows  along  in  the  path  of 
the  cab.  with  the  steering  of  its  center  of  gravity  lagging  behind  the  steering  of 
the  cab.  Furthermore,  since  the  trailer's  wheel  axle  orientation  constrains  its 
motion,  the  trailer  is  restricted  to  a  larger  turning  radius  than  the  cab  is  capable 
of  steering. 

Specially  designed  wheeled  vehicles  may  use  omni-directional  steering  [Ref. 
16].  This  rarely  used  method  allows  the  driver  to  specify  turning  rate  and 
velocity  in  any  horizontal  direction. 

Legged  vehicles  have  historically  used  similar  steering  approaches.  McGhee 
and  Iswandhi  [Ref.  17],  introduced  a  two-axis  joystick  control,  analogous  to 
articulated  steering,  in  which  one  axis  controlled  the  turning  radius  and  the  other 
controlled  forward  velocity.  Orin  [Ref.  18],  applied  three-axis  joystick  control  to 
the  Ohio  State  University  Hexapod,  a  small  laboratory  scale  walking  vehicle. 
This  allowed  forward,  lateral  and  rotational  velocities  to  be  specified  by  the 
driver,  providing  steering  control  much  like  that  experienced  in  a  helicopter.  The 
current  ASY  uses  a  similar  three-axis  joystick  control. 

Tractor-trailer  style  steering  has  not  yet  been  applied  to  walking  vehicles. 

This  approach,  which  will  be  developed  in  this  thesis,  should  give  improved  two- 
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axis  control  to  the  driver  for  moving  through  areas  of  restricted  maneuverability. 
The  driver  need  only  be  concerned  with  maneuvering  the  front  end  of  the  vehicle. 
The  body  of  the  vehicle  will  follow  along  the  proven  path  established  by  the 
footholds  used  by  the  front  pair  of  legs. 

D.  STABILITY 

The  problem  of  vehicle  balance  is  a  vital  concern  for  w-alking  machines  and 
has  been  a  focus  of  many  studies.  Legged  vehicles  may  maintain  their  stability 
using  one  of  two  methods,  static  balancing  [Ref.  19]  or  dynamic  balancing  [Ref. 
20]. 

Static  stability  is  attained  by  maintaining  the  vertical  projection  of  the 
vehicle  s  center  of  gravity  within  the  polygon  defined  by  the  supporting  legs  [Ref. 
5].  This  method  is  conceptually  simple.  It  is.  however,  only  valid  for  stationary 
or  slow  moving  vehicles,  as  it  neglects  the  effects  of  inertia  on  stability. 

Dynamic  balancing  is  a  complex  process  which  places  few'er  restrictions  on 
vehicle  velocity.  The  vehicle  may  be  allow’ed  to  momentarily  move  into  a 
statically  unstable  configuration,  so  long  as.  over  time,  an  adequate  base  of 
support  is  provided  [Ref.  20].  This  is  the  mode  of  balancing  normally  used  by 
man  and  most  vertebrate  animals.  It  remains  an  extremely  complex  process, 
however,  which  is  difficult  to  reproduce  with  legged  vehicles. 

This  model  uses  only  the  static  criteria  for  stability.  Having  the  vehicle 
limited  to  reasonable  velocities  and  the  six  legs  placed  in  alternating  tripod 


support  patterns  ensures  a  high  degree  of  stability.  To  guarantee  stability,  the 
usable  working  volume  for  each  leg  is  reduced.  Figure  2.5  shows  a  worst  case 
situation  demonstrating  that,  if  the  legs  are  confined  to  their  respective 
constrained  working  volumes,  the  vertical  projection  of  the  center  of  gravity  will 
always  fall  within  the  triangular  pattern  formed  by  the  supporting  legs.  A  further 
discussion  of  the  constrained  working  volume  can  be  found  in  [Ref.  4]. 

E.  GRAPHICS 

There  is  a  w'ide  spectrum  of  available  options  from  which  to  choose  in  the 
field  of  graphic  displays.  Decisions  are  required  as  to  running  the  simulation  on 
monochrome  or  color  monitors,  the  type  and  number  of  dimensions  for  the 
projection,  the  use  of  line  or  solid  figure  representation,  acceptable  display 
resolution  and  update  time,  and  whether  to  employ  special  hardware  options. 
State  of  the  art  graphics  machines  also  offer  possibilities  which  include  shading, 
reflectivity  of  surfaces,  and  multiple  light  sources.  A  compromise  must  be  made 
between  functionality,  visual  realism,  and  cost  in  order  to  realize  an  effective 
simulation. 

Past  simulation  models  featuring  the  ASV  [Ref.  4.21,22]  have  concentrated  on 
basic  functionality  in  the  display.  The  vehicles  and  terrain  were  represented  by 
simplified  line  drawings  on  a  monochrome  monitor.  This  study  attempts  to  take 
advantage  of  recent  developments  in  special  hardware  and  software  for  graphics 
workstations,  in  order  to  create  a  more  realistic  and  convincing  simulation.  It  is 


simulation.  It  is  believed  that  the  IRIS-2400  [Ref.  23)  represents  a  good 
compromise  between  state  of  the  art  quality,  cost,  processing  time  and 
availability.  This  system  was  therefore  selected  to  support  the  work  of  this  thesis. 


F.  SUMMARY 

This  chapter  provides  background  information  on  previous  research  leading  to 
this  study.  Discussions  include  a  brief  survey  of  examples  of  the  state-of-the-art 
w’alking  machines,  follow-the-leader  and  tripod  gaits,  vehicle  steering,  and  the 
question  of  stability  for  walking  machines.  In  addition,  several  concerns  are 
expressed  regarding  the  graphics  displays  used  to  portray  the  action  of  the 
walking  vehicles. 

The  following  chapter  contains  a  detailed  statement  of  the  ASV  simulation 
problem  to  be  solved  in  this  thesis. 
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III.  DETAILED  PROBLEM  STATEMENT 


A.  INTRODUCTION 

This  chapter  is  intended  as  a  description  of  the  nature  of  the  simulation 
modeling  problem.  In  it  is  a  discussion  of  the  configuration  of  the  ASV,  the 
mathematics  governing  its  motion,  and  foothold  selection  and  steering  algorithms 
for  two  selected  gaits.  Also  covered  are  the  simplifications  deemed  necessary  in 
the  creation  of  the  model.  The  final  section  includes  a  brief  description  of  the 
modeling  facilities. 

B.  ASV  CONFIGURATION 

The  Adaptive  Suspension  Vehicle  (ASV)  is  a  self-contained,  six-legged 
walking  machine  designed  to  traverse  uneven  terrain.  The  operator,  sitting  in  a 
cockpit  at  the  front  of  the  vehicle,  controls  the  vehicle  either  at  a  supervisory  level 
by  selecting  body  translational  and  rotational  velocities  and  allowing  the  vehicle 
to  automatically  place  the  feet,  or  by  coordinating  the  individual  legs  in  a 
precision-footing  mode.  The  various  control  modes  are  discussed  in  [Ref.  10] . 

The  vehicle  is  equipped  with  an  optical  scanning  rangefinder,  mounted  above 
the  cab.  for  short-range  sensing.  The  laser  rangefinder  has  a  range  of  10  m  and  a 
field  of  view  of  40  degrees  on  each  side  of  the  body  axis,  and  from  15  to  75  degrees 
below  the  horizontal.  [Ref.  10:  p.8] 


A  single  900  cc  four-cylinder  motorcycle  engine  is  sufficient  to  power  the  ASY 
over  sustained  periods  of  time.  This  is  possible  due  to  the  aluminum  construction 
of  the  frame  and  legs,  which  make  the  vehicle  relatively  light  (2700  kg)  for  its  size 
(3.0  m  height,  5.2  m  length)  [Ref.  10:pp.  8-10].  Power  is  distributed  to  eighteen 
hydraulic  actuator  pumps  through  an  energy  storage  flywheel  and  a  series  of 
shafts  and  toothed  belts. 

Seventeen  Intel  86/30  single-board  computers  are  used  for  onboard  processing 
and  control.  One  board  is  dedicated  to  each  leg  for  motion  control  and  leg  sensor 
data  processing.  Four  more  boards  compute  stability,  check  actuator  motion 
limits,  and  generate  leg  commands  based  on  the  operator’s  control  inputs  and  the 
internal  terrain  model.  Two  additional  boards  are  used  for  cockpit  displays  and 
controls.  The  terrain  model  is  generated  by  the  remaining  five  single-board 
computers  using  the  data  gathered  from  the  optical  rangefinder.  [Ref.  10:  pp.  8- 
10] 

The  design  of  the  ASV's  legs  features  a  two-dimensioned  pantograph 
mounted  on  a  baseplate  hinged  to  the  body  (Fig.  3.1  and  3.2).  This  design  offers 
the  advantages  of  energy  efficiency  resulting  from  decoupled  ground  reaction  force 
components,  and  simplicity  of  control  [Ref.  5,6.  and  24].  Vertical  and  horizontal 
motion  relative  to  the  baseplate  are  provided  by  independent  actuators  mounted 
to  the  plate.  Abduction  and  adduction  motion  is  provided  by  a  third  actuator 


mounted  on  the  body. 
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C.  SELECTED  WALKING  ALGORITHMS 


1.  Gaits 

The  model  used  in  this  simulation  study  currently  supports  two  styles  of 
walking  patterns.  The  first,  closely  following  that  used  by  Lee  [Ref.  4]  features  a 
periodic  tripod  forward  wave  gait.  The  second  is  a  periodic  tripod 
follow -the -leader  (FTL)  gait  [Ref.  3].  Both  gaits  have  unique  advantages  to 
offer  the  operator. 

The  great  advantage  inherent  in  the  forward  wave  gait  lies  in  the 
maneuverability  it  offers  the  walking  vehicle.  The  ASV,  operating  in  the  forward 
wave  gait  mode,  is  free  to  place  its  feet  anywhere  within  a  constrained  working 
volume  during  the  leg  placement  phase  of  the  walking  cycle  [Ref.  4:pp. 59-62]. 
This  freedom  allows  the  vehicle  great  flexibility  in  range  of  movement;  even  to  the 
point  of  permitting  turning  in  place. 

The  price  for  this  freedom  of  choice  for  leg  placement  is  that  a  foothold 
must  be  found  and  tested  for  each  time  a  leg  is  placed  on  the  ground.  In  rough  or 
obscured  terrain  the  process  of  probing  and  testing  could  occupy  virtually  all  of 
the  vehicle’s  onboard  processing  capability.  Thus,  the  vehicle’s  speed  over  ground 
could  be  severely  limited. 

In  this  type  of  terrain  the  follow-the-leader  gait  could  prove  more 
advantageous.  The  follow-the-leader  gait  requires  probing  and  testing  only  for 
the  forward  two  legs.  Since  the  following  legs  step  precisely  where  the  leading 

legs  have  gone,  no  further  searching  is  needed.  On  difficult  or  dangerous  terrain. 
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where  extensive  probing  and  testing  of  foothold  is  required,  the  FTL  gait  promises 
both  greater  ground  speeds  and  more  security. 

The  notable  disadvantage  of  the  FTL  gait  is  that  its  use  drastically 
constrains  the  vehicle's  movement.  Maneuvers  such  as  sideways  stepping  and 
turning  in  place  are  not  possible.  Turning  the  vehicle  requires  a  large  radius 
turning  circle,  similar  to  that  needed  by  a  tractor  pulling  a  long  trailer. 

2.  Steering 

The  two  walking  algorithms  utilize  different  control  schemes  matching  the 
unique  gait  characteristics.  The  forward  wave  gait  steering  mode  allows  the 
operator  to  independently  specify  longitudinal  velocity,  lateral  velocity,  and 
azimuth  angle  rate  (ideally  using  a  three-axis  joystick).  This  allows  the  operator 
to  take  fully  advantage  of  the  gait’s  maneuverability.  In  the  absence  of  a  three- 
axis  joystick  for  this  simulation,  these  body  translation  and  rotation  rates  are 
input  through  three  sliding  bar  controls  using  a  mouse-driven  cursor  on  the 
display  screen. 

The  vehicle  in  the  follow-the-leader  gait  mode,  with  its  inherent 
restriction  that  the  body  remain  between  the  two  parallel  foothold  tracks,  behaves 
very  much  like  a  tractor  and  trailer  or  a  wagon.  Just  as  the  truck  driver  steers 
the  cab  allowing  the  trailer  to  follow  in  its  path,  the  ASV  operator  in  this  mode 
steers  by  specifying  the  desired  motion  of  the  vehicle  steering  point.  This  point 
lies  just  behind  the  cockpit,  mid-way  between  the  two  front  legs,  along  the  line 
joining  the  centers  of  the  two  working  volumes.  In  the  place  of  a  steering  wheel 


and  acceleration  pedal,  the  operator  uses  a  two-axis  joystick  (simulated  with  a 
mouse-driven  cursor  on  a  steering  pad),  to  specify  the  desired  magnitude  and 
direction  of  the  cockpit's  relative  velocity  vector. 

The  truck  and  trailer  or  wagon  style  steering  commands  are  translated 
into  desired  longitudinal  and  lateral  translation  and  azimuth  rotation  rates  in 
order  to  maintain  compatibility  with  the  wave  gait  control  algorithm  in  the 
program.  This  is  done  by  first  transforming  the  steering  point  (vehicle  head) 
actual  position  and  desired  velocity  to  Earth  coordinates,  {xhE,  yhE,  zhE)  and 
(xdhE'  VdhE'  zdhE )  respectively.  The  desired  cockpit  position  (xdhE,  ydh£.  zdh£)  is 


determined  by 

XdhE  =  Xh E  +  XdhE  '  ^ 

(3.1) 

VdhE  ~  yhE  *  VdhE  '  ^ 

(3.2) 

ZdhE  r  ZhE  *  ZdhE  ' 

(3.3) 

where  At  is  the  program  display  time  increment.  Using  the  desired  cockpit 
position  and  the  centroid  of  the  middle  and  rear  legs’  footholds  (in  Earth 
coordinates)  [fht,fh  ,fhz),  the  desired  azimuth  angle  is  obtained. 

VdhE  ~ 

*d  =  tan  - —  (3.4) 

zdhE  ~ 
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The  desired  new  position  of  the  body’s  center  {xdE,UdE-  zdE)-  is  then  found  by 


L 

rdE  =  xdhE  ~  —cos9d 

2 

(3.5) 

L 

VdE  =  VdhE  ~  ”sin*rf 

2 

(3.6) 

N 

II 

*1 

(3.7) 

where  L  is  the  length  between  the  center  of  the  working  volumes 

of  the  forward 

and  rear  legs. 

The  desired  Earth  translation  rates  ( xdE  and  ydE )  and 

Euler  azimuth 

angle  rate  (*^£)  are  determined  as 

xdE  ~  XE 

XdE  ~ 

At 

(3.8) 

VdE  ~  VE 

VdE  ^ 

At 

(3.9) 

*dxE  ~ 

At 

(3.10) 

with  being  the  current  azimuth  angle.  These  Earth  and  Euler  rates  are  then 
translated  to  body  rates  ( idg ,  ydB ,  vdzB)  by 


xdB  =  idEcosq>  +  J/d£sin’1' 
VdB  =  VdE cos*  -  xdEs'm* 


(3.11) 

(3.12) 


3.  Foothold  Selection 


As  indicated  in  the  above  section,  a  new  foothold  must  be  selected  for 
each  leg  while  operating  in  the  forward  wave  gait  mode.  In  order  to  maximize  the 
foothold’s  usefulness  it  should  be  placed  so  that  the  foot  remains  in  the 
constrained  working  volume  during  the  leg's  support  phase  for  a  maximum  length 
of  time.  The  optimal  foothold  position  is  determined  as  the  "point  on  the  surface 
of  the  constrained  working  volume  such  that  [the  leg’s)  support  trajectory  is 
predicted  to  pass  through  the  foot  reference  position"  [Ref.  4:  p.100].  To  simplify 
the  computation,  the  reference  position  is  taken  as  the  center  of  the  working 
volume  and  a  straight  line  is  used  to  approximate  the  foot  trajectory.  A  line  is 
projected  opposite  to  the  direction  of  the  predicted  foot  velocity  vector  at  the 
reference  point.  The  intersection  of  this  line  and  the  boundaries  of  the 
constrained  working  volume  is  then  the  desired  foot  position.  Subsequent 
variation  of  the  body  velocity  will  alter  the  supporting  foot  trajectory,  potentially 
resulting  in  a  suboptimal  foothold. 

The  follow-the-leader  gait  foothold  selection  process  is  much  different. 
New  footholds  for  the  leading  two  legs  are  found  by  projecting  a  line  along  the 
velocity  vector  of  the  vehicle’s  cockpit.  At  a  set  distance  (1/12  the  length 
between  the  forward  and  rear  hip  joints),  along  this  line,  another  line 
perpendicular  to  it  is  projected.  This  distance  is  one  half  the  leg  stroke  of  the 
vehicle  while  operating  with  a  pitch  of  1/3  (Figure  2.2). 


REPRpDUCED  AT  GOVERNMENT  EXPENSE 


The  desired  foothold  is  determined  by  where  the  second  line  intersects  a  line 


running  through  the  center  of  the  working  volume  parallel  to  the  body's 
longitudinal  axis  (Figure  3.3). 

As  a  front  leg  abandons  its  current  foothold,  that  position  is  recorded  for 
use  by  the  middle  leg  behind  it.  In  turn,  the  middle  leg  foothold  positions  are 
saved  for  use  by  the  rear  legs.  Thus,  during  each  complete  leg  cycle,  two  new’ 
foothold  positions  are  computed.  This  compares  favorably  to  the  six  new 
footholds  needed  while  using  the  forward  wave  gait. 


37 


HSSSBSEKSss MJUI'WMIUi  HHUIIU ILVOU Ml  UUM HHVMUUII  UIKV  UI  Uf  IVWil  SiS’TJratlW 


The  current  program  allows  the  driver  to  start  operation  of  the  vehicle 
either  using  the  forward  wave  gait  or  the  follow-the-leader  gait.  Once  started,  the 
program  must  be  reset  before  switching  modes. 

D.  MODELING  SIMPLIFICATIONS 

There  are  many  simplifying  assumptions  contained  within  this  model  of  the 
ASY.  These  simplifications  were  made  largely  in  an  effort  to  speed  the 
development  of  such  features  as  the  follow-the-leader  gait.  However,  the  program 
framework  was  devised  with  future  work  in  mind.  Thus,  w’herever  possible  room 
was  left  for  generalization  and  expansion. 

The  most  notable  simplification  in  the  simulation  model  deals  with  terrain. 
The  ground  is  represented  by  a  smooth,  level,  checkerboard  pattern.  Although 
the  ASY  was  developed  to  be  able  to  traverse  unstructured  terrain,  there  are  no 
obstacles  or  obstructions  in  the  current  model.  Uneven  terrain  will  require 
inclusion  of  an  algorithm  for  estimating  the  support  plane  beneath  the  vehicle, 
foot  sensors,  and  a  new  terrain  display  routine.  A  foothold  probe  and  testing 
routine  will  also  be  needed. 

As  a  consequence  of  the  use  of  flat  terrain,  the  constrained  working  volume 
adjustments  for  uneven  terrain  (Ref.  4:  pp.  109-117]  and  body  regulation  plans  for 
varying  slopes  [Ref.  4:  pp.  87-891  were  not  required.  However,  the  basic  structure 
for  body  attitude  and  altitude  regulation  has  been  retained  in  the  program 
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modules  of  this  thesis.  Consequently,  inclusion  of  sloped  terrain  should 
necessitate  only  minor  program  changes  in  these  areas. 

The  model  contains  only  kinematic  features  of  the  ASV  operation.  This 
means  that  there  are  no  limits  on  vehicle  acceleration  imposed  by  the  model.  In 
order  to  prevent  unrealistic  performance  on  the  part  of  the  displayed  vehicle,  a 
filter  was  placed  between  the  commanded  inputs  and  the  response  of  the  vehicle. 
The  kinematics  and  filter  for  simulating  dynamic  constraints  are  described  in  the 
section  below. 

E.  MODEL  KINEMATICS 

The  kinematics  of  the  model  of  the  ASV  presented  here  closely  follow  those 
developed  in  the  computer  simulation  of  Lee  [Ref.  4{.  Body  motion  is  specified  in 
terms  of  translation  rates  along  the  body’s  forward,  lateral,  and  vertical  axes  (x. 
y,  and  z  repectively)  and  rotation  rates  around  these  axes.  The  driver  of  the 
vehicle  may  directly  or  indirectly  control  the  desired  forward  and  lateral 
translation  rates  and  the  rotation  rate  around  the  vertical  axis.  The  remaining 
three  degrees  of  freedom  are  automatically  regulated  to  maintain  a  desired  body 
attitude  and  altitude  with  respect  to  the  ground. 

Vehicle  dynamics  are  simulated  through  the  use  of  a  simple  control  filter 
inserted  between  the  ordered  rates  and  the  actual  body  rates.  As  a  result  the 


body  moves  with  a  smooth,  exponential  transition  in  response  to  driver  and  body 
regulator  control  commands. 

In  order  to  realize  these  filtered  body  rates,  the  rates  are  first  converted  to 
earth  coordinate  translation  rates  and  body  Euler  angle  rates.  Euler  integration  is 
then  performed  to  produce  translation  distances  in  earth  coordinates  and  angular 
displacement  around  the  three  body  Euler  axes. 

1.  Coordinate  Systems 

The  ASV  model  makes  use  of  two  coordinate  systems,  earth  ( x yE,  zE) 
and  body  ( xg ,  yB.  rfl),  in  its  calculations.  The  earth  coordinate  system  is  used 
wherever  it  is  required  to  specify  absolute  position  or  velocities  of  the  body,  feet, 
or  terrain.  The  earth  coordinate  system  is  defined  such  that  the  z£  axis  is  positive 
upward  and  the  unit  vectors  z£,  y£  and  z£  are  mutually  orthogonal. 

The  body  coordinate  system  is  useful  in  describing  operator  control  and 
the  coordination  of  body  and  legs.  The  origin  is  defined  as  the  center  of  the  main 
body  section  (excluding  the  cockpit).  The  zB  axis  is  projected  upward  through 
the  top  of  the  body,  while  the  zB  axis  is  forward  along  the  longitudinal  axis  and 
the  yB  axis  is  projected  to  the  body’s  left,  forming  the  transverse  or  lateral  axis. 

Earth  coordinates  are  transformed  to  body  coordinates  using  the 


relationship  of  equation  3.14. 


(3-14) 


T  T 

where  the  position  vectors  [x£,  y£1  z£.  l]  and  [xfi,  yfl,  2fl,  l]  describe  the  same 


point  in  space  in  earth  and  body  coordinates  respectively  and  H  is  a  4  x  4 


homogeneous  transformation  matrix  [Ref.  25].  The  homogeneous  transformation 


matrix  can  be  derived  by  decomposing  the  transformation  into  a  translation  from 


the  earth  coordinate  origin  and  a  series  of  rotations  about  the  Euler  axes: 


V2VJVre 


(3.15) 


The  homogeneous  transformation  matrix  T  represents  the  translation 


of  the  body’s  center  to  its  current  position  ( dz ,  d^.  dj.  The  first  rotation  about 


the  body's  vertical  axis  by  the  azimuth  angle  ¥  is  represented  by  the  matrix  T^. 


The  body  is  then  rotated  about  its  new  lateral  axis  by  the  elevation  angle,  <h.  and 


then  about  the  newly  formed  longitudinal  axis  by  the  roll  angle.  0.1 


1  Other  notations  are  sometimes  used  for  these  angles.  For  example,  in  |Ref.  19),  6  signifies 
elevation  angle  and  0  denotes  roll  angle. 


The  four  homogeneous  transformation  matrices  [Ref.  25:  p30]  are: 
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Substituting  equations  3.16,  3.17.  3.18,  and  3.19  into  3.15  yields: 


cos^cos$  cos'I'sin$sin0-sin'I'cos©  sin^sin©  ^cos'Psinfccos© 
sin^cos$  cos'Jrcos©+sin'I'sin$sin©  sin'Irsin$cos@-cos'I'sin© 
-sm$  cos$sin©  cos$cos© 

0  0  0 


2.  Body  Regulation 

A  simple  control  algorithm  is  used  in  this  and  Lee's  model  to  maintain 
the  attitude  of  the  vehicle  and  its  height  above  the  ground.  The  inputs  are  the 
estimated  support  plane  and  the  plane  formed  by  the  body's  lateral  and 
longitudinal  axes. 

Body  attitude  regulation  is  accomplished  by  rotating  the  present  body 
plane  towards  the  desired  body  plane.  The  desired  plane  can  be  expressed  as  a 
function  of  the  terrain  slope  and  be  adjusted  to  suit  the  driver3. 

The  unit  vector  Bk  along  the  rotation  axis.  (Fig.  3.4)  is  given  by 


(3.21) 


where  zB  and  zD  are  the  unit  normal  vectors  of  the  current  and  desired  body 


In  the  current  level  terrain  model,  the  desired  body  plane  angle  is  set  equal  to  zero. 


REPRODUCED  AT  GOVERNMENT  EXPENSE 


Figure  3.4  Rotation  Axis  and  Angle 


planes  and  kz  =  0.  The  rotation  angle,  -j,  is  given  by 

1  =  cos ~\zB  ■  ZD)  (3.22) 

These  values  are  used  in  the  control  function  to  obtain  the  rotation  rates  around 
the  body’s  longitudinal  axis,  u>t,  and  about  its  transverse  axis 

Body  altitude  is  defined  as  the  distance  along  the  body  plane’s  unit 
normal  from  the  estimated  support  plane  to  the  body's  center  of  gravity. 


A  mapping  function  similar  to  that  used  for  the  body  plane  can  be  used  to  relate 


the  desired  altitude.  hD,  to  the  current  terrain  slope,  h.4 
3.  Rate  Computation 

The  differential  equation  describing  the  simulated  dynamics  of  the  control 

filter  is 


y(t)  =  y(0 

T 


(3.23) 


where  r  is  the  time  constant  of  motion  and  y(t)  is  difference  between  the  desired 
and  actual  position  variable.  Integrating  both  sides  of  equation  3.10  yields  an 
exponential  response 


y{  0  =  c 


(3.24) 


The  control  filter  for  altitude  is  then 


X 

ZB  ~  ~ 


(3.25) 


Similarly  the  equation  producing  the  attitude  control  response  is 


7  =  -y. 

T 


(3.26) 


4  In  the  current  model  the  desired  height  is  set  to  a  constant  value 


The  rotational  vector  decomposes  into  rotation  vectors  about  the  forward  and 
lateral  axes,  yielding: 


1 

=  —  kS 

rt 

1 

u  =  - —  k  7. 
y  y  ' 

fj 

Velocity  is  related  to  acceleration  using  the  same  filter, 
accomplished  by  letting 

V(0  =  x{t) 

and  substituting  into  equation  3.23,  yielding: 


x(t)  =  -  —  i(t). 

T 

The  accelerations  for  the  remaining  three  rates  are 
1  . 

XB  ~  (XB  commanded  XB  current^ 

r2 

1 

yB  =  -—(VB  commanded  ~  *B  current ) 
r2 

1 

—  ^ 2  commanded  ^ z  current ) ' 

T2 

Using  the  lfirear  approximation. 

A velocity  ~  Atime- acceleration, 
the  rates  are  determined  by  equations  3.35  through  3.37. 


(3.27) 

(3.28) 

This  is 
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(3.30) 

(3.31) 

(3.32) 

(3.33) 

(3.34) 


A/ 

* B  current ) 

*B  new 

commanded 

T2 

1 B  current 

(3.35) 

At 

y B  current ) 

y B  new 

B  commanded 

T2 

yB  current 

(3.36) 

At 

—  \ 
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("M  commanded 

^  z  current 

(3.37) 

where  AMs  the  time  increment  and  r,  is  the  time  constant  of  motion. 

Body  positioning  in  this  computer  model  is  achieved  by  translating  the 
body  center  to  its  proper  earth  coordinate  position  and  then  successively  rotating 
the  body  about  its  vertical,  transverse  and  longitudinal  axes.  In  order  to  do  this, 
body  rates  are  first  transformed  into  earth  coordinate  translation  rates  and  body 
Euler  angle  rates  using  the  method  presented  by  Frank  and  McGhee  [Ref.  19]. 
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1  tan$sin©  tan$cos@ 

i> 

0  cos©  sin© 

u> 

y 

0  sec$sin©  sec$cos© 

* z 

(3.38) 


Roll,  elevation  and  azimuth  angles  and  translation  distances  are  then  found 
through  simple  Euler  integration: 

y„eu  ^  y0td  ~  v  •  *time- 


(3.39) 


4.  Leg  Kinematics 


The  ASV's  pantograph  leg  contruction  yields  relatively  simple  kinematic 
and  inverse  kinematic  equations.  These  equations  differ  slightly  from  those 
presented  by  Lee  [Ref.  4].  This  can  be  attributed  to  the  use  of  more  accurate 
dimension  measurement  than  those  assumed  by  Lee. 

For  the  front  left  leg  (leg  number  one)  shown  in  (Fig.  3.1  and  3.2).  the 
foot  position  is  given  by 


if  -  od2  *  hz  (3.40) 

y f  -  (5 /3  -!-  4<f,)sin0  +  /4 cos©  -  hy  (3.41) 

Zj  =  /4sin0  -  (5/3  -  4d,)cos©  (3.42) 

where  the  hip  position  (hx.  hy.  hj  and  foot  position  ( if .  yf.  zf)  are  given  in  body 
coordinates,  and  d2.  and  0  are  the  joint  variables. 

The  inverse  kinematic  equations  for  the  joint  variable  dv  derived  from 
equation  3.40  is 
1 

d2  =  (if  —  hx).  (3.43) 

5 


Rearranging  and  squaring  both  sides  of  equations  3.41  and  3.42  yields, 

a2sin2©  -  a  l4 sin©cos0  4  /2 cos2©  =  (j -  hy)2  (3.44) 

/4sin'0~-  a  /4sin0cos©  -  a2cos2©  -  (Zf  -  hj2  (3.45) 
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where  a  =  (5/3  -  4<f } ) .  Solving  equations  3.31  and  3.32  gives. 


-  (  5/3  -  yj {y,  -  ht 


y  -  =;  - 


0  =  sin 


a  (»/  ~  V  +  M“/  “ 


2  ,  2 
a  -r  l4 


(3.46) 

(3.47) 


In  addition  to  the  joint  parameters,  this  model  requires  leg  upper  (thigh) 
angle,  a,  the  lower  leg  (shank)  angle,  -7.  and  the  knee  position  in  body  coordinates 
(xk.  yk,  zk).  The  thigh  angle  is  given  in  terms  of  joint  variables  as 


n 

-1 

dz 

-i 

a  -  — 

2 

tan 

h  +  di 

-  cos 

/i  "  V  -  dz  +  K  +  *3)* 


I  2 


(3.48) 


and  the  knee  position  as 

ik  =  /;coso  -  ht  (3.49) 

yk  -  (/2sino  dj)sin©  *  /4cos0  +  h  (3.50) 

zk  =  /4sin©  -  (/2 sina  -  <fj)cos0  (3.51) 

The  knee  angle  is 

~  zf 

*>  =  tan  -  (3.52) 

xk~  xf 


All  six  legs  of  the  ASY  share  similar  geometries.  The  remaining 
kinematic  and  inverse  kinematic  equations  can  be  obtained  from  equations  3.40 
through  3.52  with  appropriate  sign  changes. 


F.  SIMULATION  FACILITIES 
1.  Hardware 

The  computer  simulation  presented  here  is  designed  to  run  on  either  of 
the  two  Silicon  Graphics.  Inc.  IRIS-2400  workstations  currently  in  the  computer 
graphics  laboratory’  in  the  Department  of  Computer  Science  at  the  Naval 
Postgraduate  School.  The  IRIS  (Integrated  Raster  Imaging  System)  consists  of  a 
Geometry  Pipeline,  a  general  purpose  microprocessor,  a  raster  subsystem,  a  60Hz 
non-interlaced  high-resolution  RGB  display  monitor  and  a  keyboard.  In  addition 
each  unit  has  been  equipped  with  two  72  megabyte  disk  drives,  a  cartridge  tape 
unit,  a  floating  point  accelerator,  and  a  three-input  mouse.  The  Geometry 
Pipeline  is  a  series  of  ten  or  twelve  custom  VLSI  chip  matrix  multipliers.  L'nder 
the  control  of  the  applications  grap  lies  processor,  it  performs  matrix 
transformations,  clipping  and  scaling  of  coordinates.  The  output  is  sent  to  the 
raster  subsystem  which  performs  functions  such  as  Ailing  in  pixels,  shading, 
depth-cueing  and  hidden  surface  removal. 

The  first  IRIS  system  is  based  on  a  Motorola  MC68010  processor  with  5 
megabytes  of  CPU  memory  and  a  1024  x  786  x  8  bit  display  memory.  It  is  also 
equipped  with  a  digitizer  tablet.  The  second  IRIS  system  is  a  more  capable 
Turbo-2400.  It  is  based  on  a  Motorola  MC68020  processor  and  has  4  megabytes 
of  CPU  memory  and  a  1024  x  768  x  32  bit  display  memory.  An  Ethernet  network 
connects  both  workstations  to  two  VAX  11/780's  and  one  VAX  11/750. 


2.  Software 

The  IRIS  Graphics  Library  contains  a  large  number  of  graphics 
commands  and  utilities.  This  allows  the  user  great  flexibility  in  the  choice  of 
coordinate  systems  and  display  techniques.  While  the  software  is  written  in  C. 
the  graphics  commands  may  be  called  in  C.  FORTRAN,  Pascal,  and  Lisp.  The 
code  for  the  model  presented  in  this  study  is  written  exclusively  in  C. 

G.  SUMMARY 

The  previous  sections  of  this  chapter  outlined  the  physical  constraints, 
simplifications,  and  tools  used  in  the  development  of  this  simulation.  The  next 
chapter  describes  the  operation  and  construction  of  the  actual  simulation 


program. 


m 


IV.  SIMULATION  PROGRAM 

A.  INTRODUCTION 

In  this  chapter  the  simulation  program  is  presented.  The  first  section  consists 
of  a  user’s  guide,  with  complete  instructions  on  how  to  use  each  program  feature. 
The  second  section  introduces  the  working  environment  for  graphics  on  the  IRIS- 
2400.  The  final  section  describes  the  internal  operation  of  the  simulation  program 
and  discusses  the  flow  through  the  major  modules.  A  complete  listing  of  the 
program  is  provided  in  the  appendix. 

B.  USER  S  GUIDE 
1.  Starting  Up 

The  program  walk,  c  is  relatively  simple  to  use.  It  is  entirely  menu-driven, 

with  a  single  mouse  button  and  cursor  performing  all  selection  functions.  To  start 

the  program,  type  the  command  "walk". 

Immediately  displayed  on  the  monitor  is  a  split  screen  view  of  the  control 

panel  and  the  ASV  on  its  terrain  (Fig.  4.1).  The  right  half  of  the  screen  features 

a  three-dimensional  projection  of  the  ASV  on  a  green  and  white  checkerboard 

plane  against  a  blue  backdrop.  The  user's  vantage  point  is  fixed  relative  to  the 

center  of  gravity  of  the  vehicle  (above  and  initially  to  the  vehicle's  left  side),  so 

that  the  vehicle  will  continuously  remain  in  view  while  walking. 
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View  of  Initial  Screen 


The  left  half  of  the  screen  contains  a  two-dimensional  representation  of 
the  control  panel.  Initially  it  features  only  the  six  yellow  selection  panels  of  the 
main  menu  on  a  cyan  background. 

2.  Menus 

A  menu  item  is  selected  by  placing  the  cursor  over  the  corresponding 
panel  and  clicking  the  middle  mouse  button.  Pressing  the  button  down  will  cause 
the  panel  beneath  the  cursor  to  be  highlighted  in  red,  as  a  potential  choice. 
Releasing  the  button  selects  the  highlighted  menu  item.  If  no  changes  are  desired 
in  the  current  menu  selection,  simply  move  the  cursor  to  a  portion  of  the  screen 
outside  the  menu  selection  region  and  release  the  mouse  button.  Selected  menu 
items  are  highlighted  in  bright  yellow. 

3.  Forward  Wave  Gait 

In  the  forward  wave  gait  mode,  vehicle  velocities  are  specified  in  terms  of 
body  axis  translation  and  rotation  rates.  Three  of  these  rates  -  longitudinal 
velocity,  lateral  velocity,  and  yaw  rate,  are  directly  controllable  by  the  operator. 
The  rates  for  the  remaining  three  degrees  of  freedom  are  automatically  adjusted 
by  the  vehicle  in  order  to  maintain  proper  attitude  and  altitude.  All  rates  in  this 
mode  are  defined  with  respect  to  the  body’s  center  of  gravity. 

Selecting  the  forward  wave  gait  panel  produces  a  secondary  menu 
displayed  immediately  below  the  main  menu  (Fig.  4.2).  This  secondary  menu 
contains  three  additional  panels  for  use  in  specifying  the  vehicle's  body  rates.  The 
panels  are  operated  in  the  same  manner  as  those  in  the  main  menu.  To  the  right 
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Figure  4.2  View  of  Forward  Wave  Gait  Screen 


of  the  menu  panels  are  six  simulated  LED  readouts,  used  for  displaying  the 
magnitude  of  the  curren  f  nd  ordered  rates. 

Releasing  the  middle  mouse  button  while  the  cursor  is  inside  the  bounds 
of  one  of  the  secondary  menu  panels  results  in  a  sliding  bar  control  panel  being 
displayed  on  the  left  edge  of  the  screen  (Fig.  4.3).  Velocity  commands  are  input 
by  placing  the  cursor  within  the  black  center  region  of  the  bar  control  area.  A 
yellow  bar  level  indicator  will  rise  or  fall  to  match  the  cursor  level,  indicating  the 
commanded  velocity  value.  No  clicking  of  the  mouse  button  is  required.  To  set 
the  commanded  input  at  the  desired  level,  move  the  cursor  to  the  desired  height 
and  then  slide  the  cursor  horizontally  until  it  is  outside  the  center  region  of  the 
sliding  bar  panel.  A  red  bar  level  indicator  displays  the  current  velocity  of  the 
vehicle. 

4.  Follow-the- Leader  Gait 

Control  while  in  the  follow-the-leader  gait  mode  is  achieved  by  specifying 
the  desired  relative  velocity  vector  of  the  ASV’s  steering  point.  The  operator,  in 
essence,  points  the  vector  in  the  direction  in  which  the  steering  point  should 
travel,  relative  to  the  body  longitudinal  axis.  As  stated  in  the  previous  chapter, 
the  steering  is  very  much  like  that  of  a  long  tractor-trailer  type  of  vehicle.  The 
control  algorithm  factors  in  the  magnitude  of  the  desired  velocity,  footholds  and 
current  velocity  and  automatically  regulates  the  body’s  motion. 
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Figure  4.3  View  of  Forward  Wave  Gait  Screen  with  Sliding  Bar  Control 


The  follow-the-leader  gait  control  mode  is  invoked  by  using  the  lower  left 
panel  of  the  main  menu.  When  this  panel  is  selected,  a  white  rectangular  control 
area  appears  directly  beneath  the  main  menu  (Fig.  4.4).  If  the  middle  mouse 
button  is  held  down  while  the  cursor  is  within  this  region,  the  cursor  controls  a 
simulated  two-axis  joystick.  The  vertical  axis  represents  the  magnitude  of  the 
relative  velocity  vector  and  the  horizontal  axis  represents  the  direction.  A  solid 
yellow  line  is  used  to  indicate  the  current  joystick  position,  and  thus  the  input 
values.  The  vehicle’s  actual  relative  cockpit  velocity  vector  is  indicated  by  a  solid 
red  line. 

5.  Status  and  Warnings 

The  status  menu  option  exists  to  provide  the  operator  with  numerical 
data  on  leg  and  body  position  and  movements.  Selecting  this  item  causes  a 
yellow  and  black  display  panel  to  appear  below  the  main  menu  (Fig.  4.5). 
Featured  on  this  panel  are  the  translation  and  rotation  rates  (with  respect  to  the 
body  axes),  the  position  of  the  vehicle’s  center  of  gravity  (in  Earth  coordinates), 
the  vehicle’s  orientation  (in  Euler  angles),  the  walking  cycle  period,  the  position  of 
each  foot  (in  body  coordinates)  and  the  angles  of  various  components  of  the  legs. 
The  values  are  updated  each  display  cycle. 

During  the  operation  of  the  vehicle,  checks  are  made  on  operating 
parameters.  If  a  leg  become  positioned  so  that  the  foot  is  outside  its 
corresponding  constrained  working  volume,  a  red  warning  box  is  flashed  in  the 
lower  left  corner  of  the  screen.  Similarly,  if  the  walking  cycle  period  becomes  too 
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Figure  4.5  View  of  Status  Display  Screen 


small,  a  yellow  warning  box  is  displayed.  In  addition  to  the  warning,  a 
deceleration  routine  is  activated  to  slow  the  vehicle  until  the  period  comes  up  to 
an  acceptable  level  [Ref.  4:  pp.  66-71]. 

6.  Reset  and  Exit 

The  reset  option  returns  all  vehicle  parameters,  including  position,  to 
their  original  values.  This  feature  was  included  to  save  time  when  making  a  series 
of  test  runs.  The  exit  option  ends  the  program,  clears  the  screen  and  returns  the 
user  to  the  current  UNIX  shell. 

C.  GRAPHICS  ON  THE  IRIS-2400 

Figures  are  displayed  on  the  IRIS-2400  by  calling  a  series  of  short  graphics 
commands,  called  primatives.  The  primatives  are  interpreted  into  graphical 
displays  by  the  software  and  special  hardware  of  the  IRIS  system.  These  include 
commands  for  specifying  color,  drawing  lines,  circles,  irregular  polygons,  and 
printing  text  characters  on  the  screen.  There  are  also  a  series  of  primatives 
designed  to  manipulate  coordinate  transformation  matrices  for  the  purpose  of 
scaling,  rotating  and  translating  figures. 

A  sequence  of  graphics  commands  may  be  grouped  into  a  listing  called  an 
object.  This  object  list  may  then  be  conveniently  executed  using  a  single  call. 
Once  created,  the  object  list  may  at  any  time  be  edited  as  desired  through  the  use 
of  object  tags.  The  object,  in  essence,  functions  as  a  reconfigurable  graphics 


subroutine. 


Objects  and  figures  in  this  program  are  displayed  in  the  reverse  order  in 
which  they  are  called.  The  last  object  is  overlaid  in  front  (with  respect  to  the 
viewer's  vantage  point),  of  the  previously  called  objects.  An  important  exception 
to  this  is  the  treatment  of  the  back  face  of  polygons.  The  reverse  side  of  a 
polygon,  in  the  back  face  polygon  removal  mode,  is  considered  transparent  and  is 
automatically  removed  from  the  image  as  a  hidden  surface  by  the  IRIS  hardware. 
This  feature  enables  the  display  of  more  realistic  appearing  three-dimensional 
objects. 

In  the  double  buffer  display  mode  utilized  by  this  program,  the  special  display 
memory  is  divided  into  two  sets  of  bit  plane  buffers.  As  one  buffer  is  having 
display  data  written  into  it,  the  other  is  used  to  refresh  the  monitor.  Once  the 
writing  is  complete,  the  functions  of  the  buffers  are  swapped,  and  a  new  cycle  of 
writing  commences.  This  display  mode  provides  for  a  smooth  simultaneous 
update  of  the  entire  screen. 

D.  PROGRAM  ORGANIZATION 

The  simulation  program  can  be  divided  into  three  general  sections; 
initialization,  simulation  loop,  and  termination.  The  heart  of  the  program,  the 
simulation  loop,  cycles  through  an  input  phase,  which  serves  as  the  operator's 
control  interface  for  the  vehicle;  the  calculation  phase,  in  which  the  parameters 
for  the  position  and  orientation  of  the  ASV's  body  and  legs  are  determined:  and  a 
display  phase.  The  initialization  section  performs  tasks,  such  as  defining 


coordinate  systems  and  creating  object  lists,  required  to  start  up  the  loop.  The 
termination  section  clears  the  screen  and  buffers  in  preparation  for  the  next  IRIS 
user.  A  flow  chart  featuring  the  program's  primary  modules  is  shown  in  Figure 
4.6. 

Since  the  order  in  which  objects  are  called  is  critical  in  this  display  mode, 
special  provisions  are  needed  to  create  a  full  360  degree  viewing  coverage  of  the 
maneuvering  vehicle.  Specifically,  four  separate  ASV  objects  lists  are  created  in 
the  object  construction  module  of  the  initialization  section.  Each  object  has  the 
vehicle  components  ordered  for  proper  viewing  from  one  of  four  viewing 
quadrants.  The  display  section  therefore  needs  only  to  determine  from  which 
quadrant  the  vehicle  is  to  be  viewed  and  call  the  appropriate  object. 

The  simulation  loop  is  the  dominant  part  of  the  code,  containing  the 
overhelming  majority  of  the  program  modules.  The  loop  begins  with  a  call  to  the 
driver's  command  interface  module.  This  module  controls  the  operation  and 
display  of  the  menu  system,  the  status  panel,  and  the  sliding  bar  and  joystick 
controls,  as  well  as  processing  F.T.L.  gait  steering  commands.  The  organization 
of  the  control  module  is  shown  in  Figures  4.7  and  4.8. 


Figure  4.6  Main  Simulation  Flowchart 
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Figure  4.7  Command  Interface  Module  (1  of  2) 


gu  re  4.8  Command  Interface 


Immediately  following  the  command  module  are  two  checks  on  the  program 
status.  If  the  exit  option  was  selected  in  the  command  module,  the  loop  is 
interrupted  and  the  program  enters  the  termination  stage.  The  reset  option 
causes  a  module  call  to  re-initialize  all  working  parameters. 

The  calculation  phase  of  the  loop  is  begins  with  the  support  module,  where  a 
determination  of  the  estimated  support  plane  directly  beneath  the  vehicle  is 
made.  The  position  and  velocity  of  the  ASV’s  body  is  then  calculated  in  the 
body  rates  module  (Figs.  4.9  and  4.10).  The  body  kinematics  used  in  this  module 
are  discussed  in  section  III.E. 

The  gait  period  is  next  calculated  in  the  optimal  period  module.  This  module 
uses  a  optimal  period  control  algorithm  which  considers  the  kinematic  limit  of  the 
supporting  legs.  In  this  algorithm,  a  period  is  calculated  for  each  leg.  based  on 
the  time  required  for  its  foot  to  reach  the  limits  of  its  corresponding  constrained 
working  volume.  The  minimum  of  all  of  the  supporting  leg's  periods  is  chosen  as 
the  vehicle’s  optimal  period.  No  foot  should  therefore  be  required  leave  its 
constrained  working  volume  while  supporting  the  body.  A  backward  gait  period 
is  also  computed  for  the  use  of  the  wave  gait  walking  in  the  reverse  direction. 
[Ref.  4:  pp.  63-69] 
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Body  Rates 


Figure  4.9  Body  Rates  Module  (1  of  2) 
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Figure  4.10  Body  Rates  Module  (2  of  2) 
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The  deceleration  module  checks  the  output  of  the  previous  module.  If  the 
period  is  below  the  set  threshold,  the  vehicle  is  slowed.  Each  time  the  period  falls 
below  the  minimum  value,  longitudinal  body  velocity  is  cut  ten  percent  and 
lateral  velocity  and  yaw  rate  are  cut  twenty  percent.  This  slowing  occurs  in  each 
pass  until  the  walking  period  rises  to  acceptable  Limits. 

The  leg  phase  module  is  used  to  update  the  movement  phase  of  each  leg. 
The  phase,  expressed  as  a  modulo  one  floating  point  number,  indicates  at  what 
point  the  leg  is  in  its  cycle  of  supporting,  lifting  off  from  the  ground,  being 
transferred  toward  the  desired  foothold,  and  being  placed  onto  the  ground.  The 
relative  phases  of  the  legs  in  this  simulation  are  set  to  move  the  legs  in  two,  180 
degrees  out  of  phase  tripods. 

The  foot  trajectory  module  uses  the  leg  phase  information  and  the  period  in 
calculating  the  position  of  the  feet  relative  to  the  body.  The  algorithm  is  shown 
in  the  flow  chart  of  Figure  4.11.  The  transfer  time  is  the  length  of  time  allotted 
for  moving  the  foot  from  one  foothold  to  the  next.  This  determines  the  speed  in 
which  the  transfer  is  made. 

The  phase  of  the  leg  relative  to  the  beginning  of  foot  liftoff  is  referred  to  as 

the  transfer  phase.  When  the  leg’s  transfer  phase  is  negative,  corresponding  to 

being  on  the  ground  in  a  supporting  role,  the  foot's  relative  position  is  determined 

by  the  support  trajectory  module  (Fig.  4.12).  When  the  leg's  transfer  phase  is 

greater  than  zero  but  less  than  the  liftoff-transfer  transition  value,  the  relative 

foot  position  is  returned  by  the  liftoff  trajectory  module  (Fig.  4.13).  Likewise  a 
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transfer  phase  value  between  the  two  transition  point  values  yields  a  response 
from  the  transfer  trajectory  module  (Fig.  4.14  and  4.15),  and  a  phase  value 
greater  than  the  transfer-placement  transition  value  yields  a  relative  foot  position 
calculation  from  the  placement  trajectory  module  (Fig.  4.16). 

The  foothold  selection  algorithms  contained  in  the  transfer  trajectory  module 
are  discussed  in  section  III.C.3.  Note  that  within  this  module  the  desired  end  foot 
position  is  treated  differently  in  the  follow-the-leader  and  forward  wave  gait 
modes.  The  forward  wave  gait,  with  its  high  degree  of  maneuverability,  has  a 
considerable  greater  probability  that  the  projected  ideal  position  toward  the  end 
of  the  transfer  phase  will  be  much  different  from  that  at  the  start.  Therefore  the 
desired  foot  position  in  the  case  of  the  forward  wave  gait  is  updated  on  each  pass. 
In  the  follow-the-leader  gait  case  it  is  only  calculated  during  the  first  time  through 
the  the  module. 

The  results  of  the  calculation  phase  are  the  position  and  orientation  of  the 
body  and  the  relative  position  of  each  of  the  feet.  These  values  are  used,  with  the 
inverse  kinematic  relations  derived  in  section  III.E.4,  in  the  display  phase  to 
obtain  the  rotation  angles  and  translation  distances  required  to  position  the 


ASV's  component  parts. 


Figure  4.14  Transfer  Trajectory  Module  (1  or  2) 
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Figure  4.16  Placement  Trajectory  Module 


The  ASV  object  lists  are  then  edited  and  the  updated  parameters  inserted 
into  their  corresponding  rotation  and  translation  commands.  Once  this  is 
completed,  the  display  calls  are  made  for  the  background,  the  terrain  object  and 
then  the  properly  ordered  ASV  object.  Swapping  display  buffers  completes  the 
loop. 

The  ASV  simulation  program  presented  here  consists  of  fifteen  separate  files 
linked,  together  with  the  graphics,  math,  and  standard  input-output  libraries, 
using  the  UNIX  make  utility.  The  program  files  and  Makefile  listings  are 
presented  in  the  appendix.  The  routines  were  created  in  a  modular  fashion  for 
ease  of  development  and  testing  and  to  assist  in  future  program  changes. 
Constants  are  grouped  into  a  single  header  file  walk.h,  for  convenient  reference 
and  modification. 

E.  SUMMARY 

This  chapter  describes  the  ASV  simulation  program.  The  first  section  is  a 
guide  for  the  operation  of  the  program.  It  details  the  use  of  the  menu  system  and 
the  operator  controls.  The  following  section  discusses  the  display  of  graphics  on 
the  IRIS-2400.  The  final  section  covers  the  organization  and  flow  of  the  main 


routine  and  its  modules. 


V.  SIMULATION  PERFORMANCE 

This  chapter  provides  a  brief  review  of  the  performance  of  the  ASV 
simulation  program.  The  review  is  largely  subjective  and  is  based  on  the  author’s 
experience  with  the  operation  of  the  simulation. 

A.  MODELING  FIDELITY 

The  image  of  the  vehicle  on  the  screen  appears  to  be  a  reasonable  likeness  of 
the  actual  ASV.  This  is  believed,  to  a  great  extent,  to  be  due  to  the  proper 
scaling  of  dimensions  of  component  parts,  based  on  available  blueprints  of  the 
ASV.  Details  such  as  the  leg  hydraulic  actuator  housings  and  the  optical 
scanning  radar,  mounted  on  the  cab  of  the  vehicle,  add  to  the  visual  effect.  The 
color  scheme  of  the  simulation  vehicle  has  been  altered  to  enhance  the  visibility  of 
the  vehicle  and  its  parts. 

The  walking  motion  of  the  model  is  very  similar  to  that  of  the  real  vehicle. 
This  observation  is  based  on  viewing  of  videotapes  produced  at  the  Ohio  State 
University.  A  notable  difference  is  that  the  simulation  model  is  perceived  to 
operate  at  a  much  slower  speed.  A  simulation  time  increment  of  1/ 100th  of  a 
second  yields  a  display  time  to  real  time  speed  ratio  of  1:30.  Operating  the 
simulation  with  a  simulation  time  increment  much  greater  than  1  /  100th  of  a 
second  to  compensate  for  this  causes  problems  related  to  the  optimum  period  and 
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leg  phase  modules  of  the  program.  This  leads  to  gross  errors  in  the  foot  trajectory 
planning  algorithms. 

B.  FORWARD  WAVE  TRIPOD  GAIT 

Driving  in  the  forward  wave  tripod  gait  mode  is  a  very  simple  task.  Although 
a  three-axis  joystick  would  be  prefered,  the  mouse-driven  sliding  bar  control  is 
easy  to  use  and  effective.  Switching  between  control  bars  for  forward,  lateral,  or 
rotational  control  can  be  accomplished  with  reasonable  ease. 

The  external  vantage  point  of  the  vehicle  causes  very  little  problem  for 
maneuvering  the  vehicle.  This  may  change  as  the  model's  speed  increases  and 
obstacles  are  introduced  into  the  environment. 

Overall,  maneuverability  of  the  ASV  in  the  forward  wave  tripod  gait  mode  is 
clearly  demonstrated  with  this  model. 

C.  FOLLOW-THE-LEADER  TRIPOD  GAIT 

The  follow-the-leader  tripod  gait  appears  to  work  especially  well  for  forward 
straight-line  locomotion.  Turning,  however,  is  extremely  restricted.  Preliminary 
investigations  indicate  a  minimum  turning  radius  of  18  times  the  body  length, 
using  the  constrained  working  volumes  depicted  in  Figure  2.4.  This  is  far  greater 
than  expected.  An  estimated  envelope  for  turning,  based  on  initial  simulation 
trials,  is  shown  in  Figure  5.1.  Steering  commands  falling  outside  of  this  envelope 
result  in  faults  within  the  foot  trajectory  planning  algorithms.  These  faults 
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usually  occur  within  the  first  four  degrees  of  the  turn  attempt.  Decreasing  the 
display  time  interval  extends  the  maximum  magnitude  of  the  command  envelope, 
but  only  marginally  improves  the  permitted  relative  direction  command  input. 

The  shape  of  the  steering  envelope  is  rather  unexpected,  and  as  of  yet. 
unexplained.  Factors  likely  to  have  the  greatest  influence  on  the  envelope  are  the 
geometry  of  the  leg’s  constrained  working  volume  and  the  implementation  of  the 
optimum  period  and  foot  trajectory  planning  algorithms. 

Expanding  the  constrained  working  volume  to  the  full  working  volume  has  a 
remarkable  effect  on  the  maneuverability  of  the  vehicle,  while  operating  in  the 
follow-the-leader  gait  mode.  By  doing  so,  the  minimum  turning  radius  improves 
to  approximately  five  times  the  body  length.  This  indicates  a  great  potential 
advantage  in  utilizing  dynamic  stability  algorithms  for  future  gaits. 

Overall  the  follow-the-leader  gait  and  tractor-trailer  steering  appear  to  be 
successful  for  level,  relatively  obstruction-free  terrain.  Further  research  is  needed 
to  determine  the  nature  of  the  limitations  and  the  means  to  expand  the  vehicle's 
maneuverability  while  operating  in  this  mode. 
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VI.  SUMMARY  AND  CONCLUSIONS 


In  this  thesis  a  tripod  follow-the-leader  gait  class  is  introduced  for  use  by  six¬ 
legged  walking  vehicles.  The  class  represents  an  extension  of  previously  defined 
follow-the-leader  gaits  and  should  prove  useful  for  legged  vehicles  traveling  in 
rough  or  treacherous  terrain  conditions. 

A  new  style  of  steering  is  also  developed  for  follow-the-leader  gaits.  This 
steering  mode  exhibits  a  general  response  similar  to  that  found  in  steering  a 
wheeled  tractor-trailer  vehicle.  With  this  mode,  the  driver  is  concerned  only  with 
specifying  the  velocity  of  the  front  of  the  vehicle.  The  algorithm  ensures  that  the 
body  of  the  vehicle  follows  along  the  path  of  the  front. 

An  improved  simulation  model  constructed  to  study  the  gait  and  steering 
algorithms  is  also  presented  in  this  thesis.  The  vehicle  selected  as  a  physical 
reference  for  the  model  is  the  Adaptive  Suspension  Vehicle  (ASV).  which  is 
currently  undergoing  testing  and  development  at  the  Ohio  State  University.  The 
model  developed  is  intended  as  a  general  tool  for  analyzing  a  variety  of  walking 
control  algorithms  for  legged  vehicles. 


A.  RESEARCH  CONTRIBUTIONS 

Previous  research  on  follow-the-leader  gaits  [Ref.  3],  has  concentrated  on  gaits 
that  are  temporally  oriented.  Since  foothold?,  are  used  by  the  following  legs 
immediately  after  being  abandoned  by  the  lead  leg,  this  produces  a  creeping 
motion  with  alternating  leg  and  body  movement. 

Extending  the  class  of  follow-the-leader  gaits  into  the  spatial  domain  relieves 
the  requirement  of  immediately  utilizing  a  foothold  as  soon  as  it  is  abandoned. 
This  gives  a  greater  degree  of  freedom  to  leg  movement  and  allows  the  possibility 
of  smooth,  continuous  body  motion  with  shorter  leg  stroke. 

The  nature  of  a  follow-the-leader  gait  greatly  constrains  the  maneuverability 
of  the  walking  vehicle.  The  vertical  projection  of  the  vehicle's  center  of  gravity  is 
required  to  fall  within  the  support  pattern  of  the  legs  and  is  therefore  confined  by 
the  history  of  footholds  produced  by  the  lead  legs.  The  similarity  of  this  problem 
to  that  of  a  trailer  pulled  by  a  tractor  cab  has  inspired  the  adoption  of  the  term 
"tractor-trailer"  steering.  With  tractor-trailer  style  steering,  the  driver  controls 
the  path  of  the  front  of  the  vehicle.  As  long  as  the  driver  does  not  turn  too 
sharply  (possibly  causing  a  wheeled  tractor-trailer  to  jack-knife),  the  vehicle's 
body  follows  along  this  path. 

The  selection  of  footholds  for  the  leading  legs  is  based  on  projecting  the 
relative  heading  vector  provided  by  the  operator.  The  location  of  recently 
abandoned  footholds  is  retained  within  the  control  algorithm  for  use  by  the 


middle  and  rear  legs. 


The  simulation  presented  in  this  study  models  the  kinematics  of  the  ASV. 
The  model  incorporates  many  of  the  simulation  features  presented  by  Lee  [Ref.  4l. 
including  omnidirectional  control,  automatic  body  altitude  and  attitude 
regulation,  leg  motion  planning,  body  deceleration,  and  filters  between  the  control 
inputs  and  reaction  which  provides  the  operator  with  the  "feel"  of  vehicle 
dynamics.  A  simplified  variation  of  constrained  working  volumes  is  also  used. 

The  simulation  program  has  a  modular  design  which  creates  a  flexible 
environment  for  studying  various  gaits  and  control  algorithms.  The  program  as 
currently  configured  has  two  modes  of  operation.  The  first  features  a  forward 
wave  tripod  gait  with  three-axis  control  for  steering  in  body  coordinates.  The 
second  mode  utilizes  the  follow-the-leader  tripod  gait  and  two-axis  control 
tractor-trailer  style  steering,  developed  in  this  study.  The  program's  displays  and 
controls  are  operated  with  a  mouse-driven  menu  package  using  a  single  mouse 
button. 

The  graphics  presentation  is  greatly  improved  over  Lee's  monochrome  line¬ 
drawing  representation.  Three-dimensional,  solid  body,  color  graphics  are  made 
possible  through  the  implementation  of  the  model  on  the  special  purpose  software 
and  hardware  of  the  IRIS-2400  system.  This  provides  a  notable  enhancement  of 
realism  for  the  vehicle  simulation. 


B.  RESEARCH  EXTENSIONS 


It  has  become  clear,  through  the  work  of  developing  this  study,  that  there  are 
many  directions  in  which  future  research  could  be  pursued.  Four  major  areas  to 
be  considered  for  extension  are:  quantitative  measurement  of  the  FTL  tripod  gait 
performance,  improvement  of  program  features,  improvement  of  display  speed, 
and  expansion  of  upper  level  control  algorithms  using  artificial  intelligence. 

Developing  performance  criteria  for  the  simulated  ASV  is  critical  if  one  is  to 
effectively  use  the  program  as  an  aid  for  developing  and  evaluating  walking 
algorithms  for  the  actual  machine.  Initial  research  might  well  concentrate  on 
measuring  turning  radii,  steering  reaction  times,  stability  margins  and  basic 
parameters  of  mobility. 

As  with  any  simulation  model,  there  are  many  desired  features  which  could 
be  added  to  enhance  realism.  Perhaps  the  most  important  improvement  for  this 
type  of  mobile  vehicle  would  be  the  inclusion  of  rough  or  uneven  terrain  into  the 
model.  Provisions  were  made  in  the  development  of  this  model  for  that 
eventuality.  A  few  new  algorithms,  for  functions  such  as  estimating  the  support 
plane  beneath  the  vehicle  and  adjusting  the  constrained  working  volume  to 
conform  to  the  terrain  slope,  will  need  to  be  written.  It  should  be  possible  to 
follow  the  work  of  Lee  (Ref.  4],  at  least  initially,  in  improving  the  simulation  in 
this  direction. 

Because  the  ASV  is  designed  for  rough  terrain  locomotion,  developing  a  good 

foothold  search  algorithm  is  important.  In  addition,  the  inclusion  of  foothold 
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search  into  the  simulation  model  would  enable  the  FTL  gait  to  be  better 
evaluated  with  respect  to  other  types  of  gaits  in  various  terrain  conditions.  This 
would  quantify  the  advantages  of  reduced  foothold  probing  requirements  for  the 
FTL  gait. 

This  simulation  could  also  be  used  to  further  develop  steering  mechanisms  for 
the  ASV.  Most  notably,  the  algorithm  for  the  tractor-trailer  style  steering  uses  a 
simple  method  for  body  positioning  based  on  the  centroid  of  the  established 
footholds.  A  different  method  for  body  steering  which  minimizes  the  maximum 
leg  excursions  might  improve  the  vehicle's  turning  radius. 

Dynamic  modeling  and  supplementing  the  model's  kinematics  would  greatly 
improve  the  realism  of  vehicle  movement.  Moreover,  it  should  also  notably 
increase  mobility,  as  the  vehicle  would  be  free  to  utilize  its  leg's  full  working 
volumes.  This  should  also  lead  towards  the  development  of  a  great  number  of 
new  gaits,  which  are  dynamically,  but  not  statically,  stable. 

Graphics  techniques  can  be  improved  to  enhance  the  realism  of  the  displayed 
image.  Features  such  as  shading,  depth-rueing,  reflectivity  of  surfaces,  terrain 
definition,  and  increased  vehicle  detail  are  all  possible  using  current  state-of-the- 
art  techniques.  Higher  resolution  monitors  and  an  enlarged  number  of  bit  planes 
in  the  display  hardware  are  also  highly  desirable. 

Adding  additional  features  to  the  model  has  the  decided  disadvantage  of 
requiring  more  cpu  time  for  the  simulation.  As  the  program  now  exists,  the 
simulated  vehicle  moves  and  Tacts  markedly  slower  than  the  actual  vehicle. 


There  are,  however,  ways  to  improve  the  display  time  for  the  model.  The  prime 
means  is  of  course  to  upgrade  the  hardware,  using  newly  developed  and  more 
capable  machines.  The  code  may  also  be  streamlined  for  efficiency.  Possibly 
several  of  the  interactive  features  could  be  reduced  or  eliminated. 

Another  possibility  for  improving  display  time  is  the  replacement  of  the 
integration  routines  within  the  body  kinematics  section  of  the  program  with  an 
incremental  homogeneous  transformation  matrix  technique  used  by  Lee  [Ref.  4]. 
Integration  is  used  here  because  of  the  simplicity  of  the  technique  and  the 
author’s  familiarity  with  the  IRIS-2400  special  hardware  commands  for  rotation 
and  translation.  It  may  be  that  the  homogeneous  transformation  matrix  could 
also  be  used  directly  with  the  special  hardware  to  provide  the  full  transformation 
with  fewer  trigonometric  computations.  This  possibility  has  not  been  investigated 
by  the  author. 

An  interesting  avenue  of  research  to  explore  is  to  automate  tht  upper  levels  of 
the  control  hierarchy.  It  may  be  possible  to  use  an  expert  system  shell  running  on 
a  special  purpose  LISP  machine  to  provide  driving  commands  to  this  simulation. 
As  of  this  writing,  efforts  are  underway  by  others  to  establish  communications 
between  the  IRIS-2400  system  and  a  Symbolics  3675  LISP  machine. 

Extensions  to  the  work  presented  in  this  thesis  are  possible  and  will  likely 
prove  very  fruitful.  It  is  hoped  that  this  line  of  research  will  lead  to  more  efficient 
and  practical  gaits  and  control  algorithms  for  legged  walking  vehicles  in  rough 


terrain. 
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APPENDIX 

PROGRAM  LISTING 


/******«*»*****************************«*****■******** 
This  program  is  written  for  the  iris-2400 
walk.c 


This  is  the  main  program  for  the  simulation. 
Relle  Lyman  04  May  1987 


/ 


♦ include  "gl.h" 

♦  include  "device. h" 

♦  include  "walk.h" 

♦  include  <stdio.h> 

♦  include  <math.h> 

main() 

{ 

Object  machineobject|4!.leg  7j|4  .textobj.vertextobj  ,thighobjf7'[2j[4j, 
actuatorobj  7|(2|(4|,shinobj17j:2|j4|,  walker  4j,groundobject; 

/*  NOTE:  this  program  uses  only  elements  1-6  of  arrays  and  vectors. 

Legs  are  numbered  to  remain  consistent  with  original  research  .  */ 

Tag  transrot  tag|4|,tr  end  tag[4j.  legmovetag  7  |4  . 
actmovetag]7jl2  4l.bodvtagl|4', 
thighmovetag  7|  2  |4  ,shinmovetag|7j.2]j4;; 

Colorindex  wmask  : 

int  ij.k.n. 

program  status.  /'*  desired  status  of  program:  RUN.  HALT  or  RESET  */ 
selected  gait,  /“  indicates  which  tripod  gait  is  to  be  used  */ 
slow  flag,  i*  flag  indicating  deceleration  is  needed  */ 

warning,  flag  indicating  supporting  leg  is  outside  of  working  volume  */ 

leg _stat us[ 7 ] ;  /*  status  of  leg  (supporting,  liftoff,  transfer,  placement)  */ 

static  float 

h.x|7j=  {0,155.,155.,0.,0.,-155.,-155.},  /*  Leg  attachment  points  */ 
hy  7;  =  {0,50., -50., 50., -50., 50., -50.}, 
hi[7|=  {0,23., 23., 23.,  23.. 23., 23.}, 

14  7  =  {0.L4.-L4,L4.-L4.L4,-L4}, 


static  Angle  theta  7 j  =  { 0, 0.0, 0,0, 0,0}.  ,*  Leg  component  angles  * 

alpha  7!  =  {364, 364, 364. 364.364. -364. -364  } . 
gammai7|  =  {317, 317, 317, 317, 3 17  -317.-317}; 


i  *  walk.c 


float  temp. tempi. temp2.temp3, top, bottom,  /*  Temporary  variables  ’  | 

alpharad  7,.  *  Leg  component  angles  in  radians  *  ■ 

thetarad. 

legcoord_x;7  ,  /*  Foot  position  in  leg  coordinates  */ 

legcoord  y ;  7  . 

Iegcoord_z|7  . 

azimuth, elev, roll,  /*  Body  Euler  angles  (rads)  */ 

ordered _vel_mag,  j*  Ordered  velocity  of  the  cockpit  (magnitude)  * 

ordered  vel  dir,  j*  Ordered  velocity  of  the  cockpit  (direction)  * 

dlj7j,  /*  Joint  variables  */  | 

d2|7i. 

knee|7)[2!,  /*  Relative  position  of  knee  */  | 

foot j7 , [2] ,  /*  Relative  position  of  foot  */  j 

h[4] [4],  /*  Homogeneous  transformation  matrix  */ 

invh(4j[4],  /*  Inverse  homogeneous  transformation  matrix  *, 

legphase!7  ,  /*  Phase  of  individual  legs  *  ' 

rel  legphase(7|,  *  Phase  of  individual  legs  relative  to  one*  i 

period,  /*  Period  of  leg  cycle  * f  , 

min  period.  *  Minimum  allowed  period  * 1  ' 

tx,ty.tz:  '*  Earth  coordinates  of  body  position  * 

vector  rot  rate.  '*  Body  rotation  rates  */  1 

trans  rate.  /*  Body  translation  rates  */  . 

ordered  rate,  /“  Ordered  lateral  and  longitudinal  translation  and  yaw  rales  * 
footpos(7],  *  Position  of  foot  in  earth  coordinates  *  ' 
b_footposl7j,  /*  Position  of  foot  in  body  coordinates 
fh  7 ) ,  /*  selected  footholds  (earth  coordinates)  *  ' 

oldfh  7  ;  /*  old  selected  footholds  (earth  coordinates)  *  -  j 

work  vol  cwv[7j;  /*  Constrained  working  volumes  *,  | 

plane  spe;  I*  Estimated  support  plane  */  I 

l 

/*  Initialize  the  IRIS  graphics  */  ^ 

gin  it  ( )  ;  /*  standard  IRIS  graphics  initialization  */  ^ 

doublebuffer()  ;  /*  double  buffering  mode  */  | 

gconfig()  ;  '*  configure  the  IRIS  (use  the  above  commands  * '  ¥ 

wmask-(1<  <getplanes())-l  ;  /*  enable  all  the  bit  planes  for  writing  */  | 

/  *  set  to  2**(getplanes())minus  one  I 

all  bit  planes  on  *  •  V 

writemask(wmask)  ;  J 

) 

backface(TRLE);  '*  set  backface  polygon  removal  on  *,  [t 
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qdevice) MIDDLEMOl'SE);  *  set  up  the  queue  for  the  menu  *, 
ue(MlDDLEMOl  SE  MOrSEX. MOISEY) 

mapcolor(LTYELLOVA  .225,225.0).  *  create  new  colors  *' 

mapc<.|»r(  WH1TE1 .230.230.250); 

view  port  1 4 10. 1023.0,767)  .  *  set  world  view  * 

per* pecu ve( 600, 1 6 H  O  76«).0  0.1023  0)  . 

make  the  ground  * 
makeground)  Agroundobject ); 

*  make  the  robot 

makewalker(machmeobject ,dl ,d2. theta. knee. gamma. alpha. transrot  tag. 
tr  end  tag. walker. leg. thighobj. act uatorobj.shinobj, 
legmovetag.thighmovetag.actmovetag.shinmovetag.tx.ty.tz.roll. 
elev  azimuth. hx.hy.hz.l4)  ; 

"  In  itialize  the  ASV  walking  routine  parameters.  */ 
inn  ialize(h.invh.Arot  rale. Atrans  rate,  Aordered  rate.Aspe.Aperiod. 
leg  status. legphase.rel  legphase,  footpos.b  footpos.cwv.fh. 
oldfh. Aselected  gait, Aorderedvelmag. Aordered  vel  dir. 

A  min  period. A  program  status.Atx.Aty.Atz.Aroll.AelevAazimuth); 

while(TRl'E)  *  Main  program  loop  * / 

< 

’  Input  the  driver's  commands.  */ 

driver  command) Aordered  rate, Arot  rate, Atrans  rate.Aprogram  status, 
b  foot  pos. A  period. alpha. gamma, theta. Aslow  flag.Aroll.Aelev, 
Aazimuth.Aix.Aty.Atz.Aorderedvelmag, Aordered  vel  dir,fh, 
Aselected  gait); 

if  (program  status  ==  HALT) 

{ 

,m  Quit  program.  */ 
break; 

> 

if  (program  status  ==  RESET) 

{ 

Reinitialize  the  ASV  walking  parameters.  *  ' 
init  ialize(h.invh.Arot  rate,  Atrans  rate,  Aordered  rate.  Aspe,  A  period, 
leg  status. legphase.rel  legphase,  footpos,b  footpos.cwv.fh 
oldfh. Aselected  gait, Aordered  vel  mag, Aordered  vel  dir. 

A  min  period, Aprogram  status.Atx.Aty.Atz.Aroll.Aelev, A  azirnut  h  I 

> 


D-A181  MS 
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/*  walk.c  *, 


Calculate  the  estimated  support  plane.  “/ 

Future  revision  needed  for  rough  terrain.  */ 
support  plane(&spe); 

/*  Calculate  the  body  rotation  and  translation  rates.  */ 
body  rates(&rot  rate,&trans  rate,&spe,h.invh. A; ordered  rate, 
&tx,&ty.&tz,&roll,&elev,&azimuth): 

Calculate  the  constrained  working  volume  for  the  legs.  */ 
con  _work_vol(cw  v,b_footpos, leg  _status,& warning); 

I*  Calculate  the  optimal  period  for  walking.  */ 
optimal  period  (legphase.bfootpos.&rot  rate.&trans  rate.cwv, 
leg  stat us. ti period ); 

i*  Decelerate  if  necessary. 

decelerate) &trans  rate. trot  rate.&period.&slow  flag, &min  period); 

/*  Calculate  the  phase  of  each  leg.  *  / 
leg  phase(legphase.rel  legphase.lt period); 

/*  Calculate  the  new  position  for  each  foot.  */ 

foot  trajectory (legphase.flt period, leg  status,footpos,b  foot pos.fh.oldfh, 
invh.h  cwv.&trans  rate, J: rot  rale. & selected  gait); 


,  *  Display  the  ASV  on  the  screen.  * 

.  *  This  section  computes  the  new  parameters  to  position  the  legs 
relative  to  the  body,  based  on  the  relative  position  of  the  feet. 

It  then  check  to  ensure  that  no  actuator  positions  exceed  the  limits.  *  / 

/*  Convert  foot  position  to  leg  coordinates.  */ 
for(i=l;  i<5;  i+-) 

{ 

legcoord_x|i|  =  b_footposii|.x  •  hxji]; 
legcoord_y[ij  =  b  footposlij.y  *  hyjij; 
legcoordzji,  =  b  footposji  .1  -  lnii|; 

} 

*  The  foot  position  of  the  rear  legs  are  changed  to  compensate  for 
the  180  degree  rotation  used  in  the  leg  construction  routine.  */ 
for) i  —  5;  i< 7;  i-*  -  ) 

< 

legcoord_x|ij  =  hx  i  -  b  footposji  ,x; 
legcoord  y[ij  =  b_footposjij.y  -  hy  i;; 
legcoord  1  1  -  b  footpos  i  .i  -  hz  i!; 

> 


95 


/*  walk.c  */ 


for(i=l;  i<7  ;  i-t--1-) 

{ 

/*  generate  required  parameters  dl,d2,  theta  */ 

d2[i!  =  legcoord_x(ij/5.0; 

temp=  legcoordyji]  *  legcoord_y|i|; 

temp2=legcoord_z[ij*legcoord_s|i|; 

d  1  [i]  =  -{5.0*L3-sqrt(temp+temp2-L4*L4))/4.0; 

templ=5.0*L3-t-4.0*dl(ij  ; 

switch  (i) 

{ 

case  1: 
case  3: 

case  5:  temps  =  templ*legcoord_y|i]  +  L4*legeoord_z[i]; 

break; 
case  2: 
case  4: 

case  6:  tempS  =  templ*legcoord_y[i|  -  L4*legeoord  z[i); 

} 

thetarad  -  asin(temp3/(templ*templ  -t-  L4*L4)); 
theta[ij  =  thetarad  *  573  +  0.5; 


for(i=l  ;i<7  ;  i+  +  )  /*  prepare  parameters  for  graphics  */ 

{  update  on  all  6  legs  * / 

temp  =  L3+dlji;  ; 
tempi  =  d2ji  *d2[i]  temp*temp; 

temp2  =  (Ll*Ll  -  L6*L8  +  templ)/(2.0*Ll*sqrt(templ)): 

alpharad|i]=((PI/2)-atan(d2!i]/temp)-acos(temp2))  ; 

/*  Note:  One  half  of  a  degree  has  been  added  to  all  angles  */ 
alpha[i]  =  (alphar  ad  ji]  *573+  5); 

kneeiij(0]=(L2*cos(alphwad|ij)-t-.5);  /*  relative  to  baseplate  */ 

kneeji j j  1  j  =  -((L2*sin(alpharad;i|)-  dl[i])+0.5);/*  relative  to  baseplate  */ 

foot(i)(0]=  (5.0*d2|i]  +  .5);  /*  relative  to  baseplate  */ 

foot [ij [  1  j=  *(5.0*L3+4.0*dl[i]-t-.5)  ;  /*  relative  to  baseplate  */ 
top=(knee|i]  0|-footji](0j); 
bottom =(knee[i  ■  [  1  ]-foot[i]  1 1  j); 

gamma  ij=(atan(top/bottom)*573+.5)  ; 
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*  walk.c  * l 

for  (n=0;  n<4;  n+~)  /*  The  walker  is  updated  in  each  quadrant  * 

{ 

editobj(thighobj[ij|Ojjn|)  ;  edit  each  Jeg  to  new  * / 

objreplace(thighmovetagjijjOj[nj)  ;  /*  location  */ 
rotate(alpha|i),’Y’)  ; 
closeobj()  ; 

editobj(thighobj[i|jlj|nj)  ; 
objrepiace( t highmovet ag  ji j  [  1  j  jn  j )  ; 
translate(0.0,0.0,dl|i|)  ; 
closeobj()  ; 

editobj(actuatorobj!ij[Oj|n])  ; 
objreplace(actmovetag[i  |0j [n] )  ; 
rotate(alphaji|,’Y’)  ; 
closeobj()  ; 

editobj(actuatorobjii;|l|[nj)  ; 
objreplace(actmovetag[i  |l  jnj)  ; 
translate(d2  i  ,0.0.-L3)  ; 
closeobjO  ; 

editobj(shinobj|i||0{(n|)  ; 
objreplace(  sh  in  mo  vet  ag  [  i  J  0;  j  n  ] )  ; 
rotate(gamma|i|,’Y’)  ; 
closeobj()  ; 

editobj(shinobj[ij[l|[n|)  . 
objreplace(shinmovetag[i|j ljjn])  ; 
translate((float)(knee[i][Ojj,0.0,(float)knee[i!|l])  ; 
closeobj()  ; 

editobj(leg  i’(nj)  ; 

°bjreplace(legmovetagji]jn])  ; 
rotate(theta  i  ,’X’)  ; 
closeobj()  ; 

}  /*  end  quadrant  loop  *  / 

}  /*  end  for  leg  loop  i=l  ...  */ 

For  (n=0;  n<4;  n++) 

{ 

editobj(machineobjecti'nj)  ; 
objdeletej transrot _tag[n|,tr  end  lag  n  ): 
objinsert(transrot  tag|nj); 
translate(tx,ty,ti); 
rotate((int)  (a*imuth*57S),'Z  ); 

rotate)  (int  )(ele>  *573), ’Y): 
rotate)  (int){roll*573),’X’); 

closeobj))  : 

}  /*  end  of  quadrant  loop  */ 


*  walk.c  ■/ 

set  up  the  background  */ 

color(BLUE): 

clear(); 

/*  Keep  the  viewing  relationship  constant.  */ 
perspective{600, (614.0/768), 0.0, 1023.0)  ; 
lookat(800.0-tx,800.0+ty , 550.0, tx,ty, -50.0, 1100); 

/*  CALL  THE  GROUND  •/ 

callobj  ( ground  object ) ; 

/*  Display  the  ASV  in  the  correct  quadrant  configuration  * 
if  (azimuth  <  0.0) 

{ 

{ 

azimuth  +=  2.0  *  PI: 

} 

if  (azimuth  >  2.0  *  PI) 

{ 

azimuth  -=  2.0  *  PI; 

} 

if  (azimuth  <  0.25*PI) 

{ 

callobj(machineobject!0j); 

} 

if  ((azimuth  >=  0.25*PI)&&(azimuth  <  0.75*PI) ) 

{ 

callobj  (mac  hineobject  |S]); 

} 

if  ((azimuth  >■=  0.75*PI)&&(azimuth  <  I.25*PI)) 

{ 

c  allobj  ( m  ac  hineobject  j  2] ) ; 

} 

if  ((azimuth  >=  I.25*PI)&&(azimuth  <  I.75*PI)) 

{ 

callobj  (mac  hineobject  ( 1  j) ; 

} 

if  (azimuth  >=  I.75*PI) 

{ 

callobj(machineobject[0]): 

} 

swapbuffersQ  ; 

} 


.  *  end  of  main  program  loop  */ 


/*  Clean  up  the  screen.  */ 


color(BLACK)  ; 

clear()  ; 

swapbuffers(); 

color(BLACK); 

clear(); 

swapbuffers(); 

finishj)  •' 


}  /*  END  OF  MAIN  PROGRAM  */ 


/ 


This  is  the  header  file  for  the  program  walk  c. 
walk.h 


Relle  Lyman 
14  May  1987 

*** . * . 7 


♦define  BETA 

0.5 

♦define  DELTA  TIME 

0.010 

♦define  TIME  CONSTANT 

1  0.1 

♦define  TIME  CONSTANT' 

2  0.25 

♦define  TIME  CONSTANT 

3  0.5 

♦define  FTL  GAIT 

1 

♦define  FWD  WAVE  GAIT 

2 

♦define  FORWARD 

1 

♦define  BACKWARD 

0 

♦define  END  LIFT  PHASE 

0.2 

♦define  BEGIN  PLACE  PHASE  0.8 

♦define  SUPPORTING 

0 

♦define  LIFTOFF 

1 

♦define  TRANSFER  FORWARD  2 

♦define  PLACEMENT 

3 

♦define  ON 

1 

♦define  OFF 

0 

♦define  LENGTH 

310.0  /*  The  length  between  the  forward 

and  aft  hip  joints  */ 

♦define  HALFLENGTH 

155.0  /*  Half  the  length  between  the  forward 

and  aft  hip  joints  */ 

♦define  FOOTLIFTHEIGHT 

40.0 

♦define  LONG  TIME 

1000000 

♦define  HO 

180.0  /*  Desired  body  height  (cm)*/ 

♦define  OUTER  LIMIT 

6.08  /*  cm/sec  */ 

♦define  INNER  LIMIT 

1.52  /*  cm/sec  */ 

♦define  RUN 

0 

♦define  HALT 

1 

♦define  RESET 

2 

♦define  NORMAL 

0 

♦define  SLOW 

1 

♦define  PI 

3.14159 

♦  define  UP 

1 

♦define  DOWN 

2 

♦define  IN 

1 

♦define  OUT 

0 

♦define  LTYELLOW 

100 

♦define  WHITEl 

107 

♦  define  TEXTCOLOR 

BLACK 

♦define  NOHIGHLIGHT 

LTYELLOW 

♦define  ACTIVEHIGHLIGHT  RED 
♦define  IN  ACTIVEHIGHLIGHT  YELLOW 
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/*  walk.h  \ 


#define  Ll  20.0 

#define  L2  102.0 

#define  LS  24.0 

f define  L4  32.0 

#define  L8  30.0 


struct  mag  in  xys  /*  magnitude  along  x,  y,  and  i  axes  */ 

< 

float  x,y,s; 

}; 

typedef  struct  mag  in  xyz  vector; 

struct  plane  coefficients  /*  plane  coefficients  *  ' 

{ 

float  a,b,r,d: 

}; 

typedef  struct  planecoefficients  plane; 

typedef  struct 

{ 

float  min, 
max. 
center; 

}  dimensions; 

typedef  struct 

{ 

dimensions  x, 

y> 

*; 

}  work  vol; 


typedef  struct 

{ 

int  left, right, top,bottom,x0,y0.xl,yl,x2,y2; 
char  *text0.*textl.*text2; 

}  menubox; 
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This  is  a  function  for  the  iris  2400  program  walk.e. 
init.r 

Relle  Lyman  27  Apr  1987 


♦  include  "gl.h" 

♦include  "device. h" 

♦  include  "walk.h" 

initialise! h . invh, rot _rate,trans_rate.ordered_rate.spe, period, leg  status, 
legphase. re! _!egphase,footpos,b_footpos.cwv,fh,oldfh, selected  gait, 
ordered _vel  mag, ordered _vel  dir, program  status,tx,ty  .tz.roll.elev. azimuth) 

/*  This  function  computes  the  body  rotation  and  translation  rates.  * 

vector  *  rot  rate.  /*  rotation  rate  */ 

'trans  rate.  *  translation  rate  '/ 

'ordered  rate.  /*  ordered  x  translation,  y  translation, 
and  z  rotation  rates  */ 

fh j 7  .  /'  selected  footholds  (earth  coord.)  */ 

oldfh|7l,  old  selected  footholds  (earth  coord.)  V 

footpos|7 i.  *  position  of  the  foot  in  earth  coord.  */ 

b_footpos|7];  /*  position  of  the  foot  in  body  coord.  '/ 

plane  *spe;  •'*  support  plane  in  earth  coord  */ 

work  vol  cwv|7’;  /*  constrained  working  volume  * 

float  h(4j!4  ,  /*  homogeneous  transformation  matrix  *  ' 

invh[4](4;,  /*  inverse  of  transformation  matrix  */ 

legphase[7j,  /*  phase  of  the  phase  *  / 
rel_legphase!7],  /*  phase  of  the  leg  relative  to  leg  one  */ 

'period,  /*  body  support  period  */ 

*tx,*ty  ,'tz,  /*  position  of  body  in  earth  coordinates  *  1 

'roll, *elev, 'azimuth,  /*  body  euler  angles  '/ 

'ordered  vel  mag.  /*  ordered  velocity  of  the  steering  pt  (magnitude)*/ 
'ordered  vel  dir:  /*  ordered  velocity  of  the  steering  pt  (direction)*,' 

int  leg  statusj7j.  ,  *  status  of  the  leg  * 

'program  status.  /'  desired  status  of  program  */ 

'selected  gait:  /*  type  of  tripod  gait  to  be  used  */ 

{ 

int  i,j; 


float  modulus_one();  /*  modulus  one  fund  ion  */ 


/*  initialize  the  transformation  matrix  * , 
h  0]  0  =1.0; 
hiOj  1  =  0.0; 
h;0j  2  =  0.0; 

hiojiS!  =  0.0; 

hj  1 1  jOj  =  0.0; 

h'lj  [1;  =  1.0; 
h  lj  2  =  0.0; 
h  lj  5;  =  0.0; 

h  2j  0  =  0.0; 
h  2)1"  =  0.0; 

h  2,  2i  =  1.0; 

hi 2] ' Si  =  HO;  /*  initial  height  of  the  center  of  the  body  *  / 

h|3|;0)  =  0.0: 
h  3/1.  =  0.0; 
h  3]  2  =  0.0: 

h  3 1 ;  3 1  =  1.0: 

7*  initialize  the  inverse  transformation  matrix  */ 
for  (i=0;  i<3:  i-r  — ) 

{ 

for  (j=0;  j<3;  j  +  +  ) 

{ 

invhji]|j]  =  h[j;ji]; 

} 

invh|3|[ij  =  0.0; 

invhli  [3!  =  -(h!0][il’*h‘0][3]  +  hr l]|i]*h[ lj[3  J- 
h[2)[i|*h(2|[3j); 

} 

invh{3]  [3]  =  1.0: 

/*  initialize  the  body  rotation  and  translation  rates  */ 

rot  rate- >x  =  0.0; 

rot_rate->y  =  0.0; 

rot  rate->z  =  0.0; 

trans  rate- >x  =  0.0; 

translate- >y  =  0.0; 

trans  rate->z  =  0.0; 


/*  initialize  the  commanded  body  rates  */ 
ordered  rate- >x  =  0.0;  ,'*  translation  *, 
ordered  rate- >y  =  0.0;  translation  * , 
ordered  rate- >z  =  0.0;  /*  rotation  *, 


♦period  =  LONG  TIME; 

♦selected  gait  =  FWD  WAVE  GAIT; 
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J 


'*  init.c  */ 

/*  initialize  the  relative  leg  phase  *  / 

rel  legphasejlj  =  0.0; 

rel  legphase[2!  =  0.5; 

rel  legphasejs)  =  BETA; 

rel_legphase[4|  =  BETA-0.5; 

rel  legphase|5j  -  2*BETA  -  1.0; 

rel  legphase|6|  =  modulus_one(2*BETA  -  0.5); 

/'*  initialize  the  leg  status  and  phase  */ 
for  (i=l;  i<7;  i — h) 

{ 

leg  statusji]  =  SUPPORTING; 
legphaseii  =  rel  Jegphaseii]; 

} 

/*  initialize  the  constrained  working  volume  for  each  leg  * [ 
cwv[l].x.min  =  95.0; 
cwvilj.x.max  =  215.0; 
cwvjlj.x. center  =  155.0; 

cwv  lj.y.min  =  60.0: 

cwv  lj.y.max  =  131.0: 
cwvll  j.y.  center  =  95.0; 

cwv  li.z.min  =  -240.0; 
cwv  lj.z.max  =  -  80.0; 
cwv:  1  ].z. center  =  -160.0; 

cwv[2].x.min  =  95.0; 
cwvj2|.x.max  =  215.0; 
cwvj2j.x. center  =  155.0; 

cwv[2|.y.min  =  -131.0; 
cwv;2|.y.max  =-60.0; 
cwv:2).y.center  =  -  95.0; 

cwv|2).z.min  =  -240.0; 
cwv  2i.z.max  =  -  80.0; 
cwv  2j.z. center  =  -180.0; 

cwv  Sj.x.min  =  -  60.0; 
cwv'3|.x.max  =  60.0; 

cwv  3l.x. center  =  0.0; 

cwvSi.y.min  =  60.0; 
cwv.Sj.y.max  =  131.0; 
cwv|3].y  .center  =  95.0; 

cwvj3|.z.min  =  -240.0; 
cwv|3l.z.max  =  -  80.0; 
ewv|3|.z. center  =  -160,0; 
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'*  init.c  * j 

cwv>4  .x.min  =  -  60.0; 

ewv  4  .x.max  =  60.0; 

cwvi4  .x. center  =  0.0; 

cwv|4  .v.min  -  -181.0; 

cwvj4  .y.max  =  -  60.0; 

cwv;4.y. center  =  -  95.0; 

cwvi4. z.min  =  -240.0; 
cwv  4|. z.max  =  -  80.0; 
cwv[4.z. center  =  -160.0; 

cwv|5j. x.min  =  -215.0; 
cwv|5. x.max  =  -  95.0; 
cwv;5.x. center  -  -155.0; 

cwv|5’. v.min  =  60.0; 
cwv]5  .y.max  =  131.0: 
cwv  5  .y. center  -  95.0: 

cwv!5  .z.min  -  -240.0; 
cwv5  .z.max  =  -  80.0; 
cwv  5  .1. center  =  -160.0; 

cwvi6  x.min  =  -215.0; 
cwv  6  .x.max  =  -  95.0; 
cwv|6\x. center  -  -155.0; 

cwv|6iy.min  =  -131.0; 
cwvj6  .y.max  =  -  60.0; 
cwv6!.y. center  =  -  95.0; 


cwv|6  .z.min  =  -240.0; 
cwv;6  .z.max  =  -  80.0; 
cwv;6:.z. center  =  -160.0; 


/*  initialize  the  selected  foothold  positions  *  ' 
fh  1 ; . x  =  cwv| ll.x. center  -+  LENGTH/12.0; 
fhi2|.x  =  cwv|2j.x. center  -  LENGTH /12.0; 
fhSj.x  -  cwv(3j.x. center  -  LENGTH '12.0; 
fh  4|.x  =  cwv|4].x. center  +  LENGTH/12. 0; 
fh,5j.x  =  cwvj5j.x. center  4  LENGTH/ 12.0; 
fh  6|.x  =  cwv|6:.x. center  -  LENGTH/12.0; 


for  (i- l;i<  7:i~ 

{ 


fh  ij.y  -  cwv  i,.y. center; 
fh  i  .z  -  0.0; 
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*  init.c  * 

/  ’  initialize  the  earth  relative  foot  positions  * 
for  (i=  l;L<7;i-r  +  ) 

\ 

footposji  x  =  fhjij.x: 
footposjii.y  =  fhjij.y; 
footposji!. z  =  fhji  .z; 

} 

/*  initialize  the  old  selected  foothold  positions  */ 
for  (i=  l;i<7;i-t--r) 

{ 

oldfhi’.x  =  fh  i  .x  -  LENGTH/3.0; 
oldfh  i|.y  =  cwvji.y. center; 
oldfhiij.z  =  0.0; 

} 

/  *  initialize  the  body  relative  foot  positions  *  1 
for  (i=l;i<7;i-t-  +  ) 

{ 

b  footposji). x  =  cwv  i  x. center; 
b  footposji). y  =  cwv  i  .y. center; 
b  footposji). z  =  cwv  i  .z. center; 

} 

/*  initialize  the  estimated  support  plane  */ 

spe-  >a  =  0.0, 

spe->b  =  0.0; 

spe->c  =  1.0; 

spe->d  =  0.0; 

/*  initialize  the  ordered  velocity  of  the  steering  point  */ 
‘ordered  vel  mag  =  0.0; 

‘ordered  jvel_dir  =  0.0; 

,  *  initialize  the  body  attitude  and  position  */ 

‘roll  =  0.0; 

*elev  —  0.0; 

‘azimuth  -  0.0; 

*tx  -  0.0; 

*ty  -  0.0; 

*tz  -  HO; 

,  *  initialize  desired  program  status  */ 

‘program  status  -  RUN; 

}  /*  end  of  initialize  */ 
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This  is  a  function  for  the  iris  2400  program  walk.c. 
driver,  c 


Relie  Lyman  IS  May  1987 

«*•«**»•*•***••***••**•*•***•*•*•••***>• »***»«**«******,*»» ***»»**»*y 

^include  "gl.h" 

^include  "device. h" 

#include  "walk.h" 

^include  <stdio.h> 

^include  <math.h> 

menubox  box|]  =  { 

0.0.0, 0, 0,0,0, 0.0, 0,»not")"used", "here", 

100. 200. 670. 525. 120.567. 120. 597.120. 627,  "GA1T","WAVE","FWD", 

200. 300.670.525. 220.567.220. 597.220.627,  "ATTITUDE", "AND", "ALTITUDE", 

300.400.670.525.320.567.320.597.320.627, "  "."RESET","  ", 

100.200.525.380.120.422. 120.452. 120. 482,  "GAIT","  ","FTL", 

200. 300. 525. 380. 220.422. 220. 452. 220. 482,  "REPORT","  "."STATUS", 

300. 400. 525. 380.320.422. 320. 452. 320. 482,  "PROGRAM","  "."EXIT", 

100.200. 310.230.120.250. 120. 270. 120. 290,  "REVERSE". "FOR  WARD". "TRANSLATE". 

100.200.230.150.120.170. 120. 190. 120. 210,  "RIGHT". "LEFT", "TRANSLATE", 

100, 200. 150.70.120, 90,120, 110, 120, 1S0."RIGHT". "LEFT", "ROTATE". 

100.200.310.230.120.250.120.270.120.290. "  "."  ", 

100.200.230.150.120.170.120.190.120.210, "  "."  ", 

100,200,150.70,120,90,120,110,120,130,"  "}; 

driver  command(ordered  rate. rot  rate.trans  rate, program  status, 
b  footpos, period, alpha, gamma, theta, slow  flag, roll, elev, 
azimuth, tx.ty.tz.ordered  vel  mag, ordered  vel  dir.fh, selected  gait) 

/*  This  function  inputs  the  driver’s  commands  using  a  menu  and 
the  mouse.  */ 

vector  ‘ordered  rate,  '*  ordered  x  translation,  y  translation,  and  z  rotation  rates  */ 
‘rotrate,  /*  actual  rotation  rate  vector  */ 

‘trans  rate,  /*  actual  translation  rate  vector  */ 
b_footpos(7j,  /*  position  of  foot  in  body  coordinates  */ 
fh(7  ;  /*  selected  footholds  (in  earth  coordinates)  */ 

int  ‘program  status,  /*  desired  status  of  the  program  RUN/HALT  ^ESET  */ 

‘selected  gait.  /*  desired  tripod  gait  */ 

‘slow  flag;  /*  flag  indicating  deceleration  is  required  */ 

float  ‘period,  /*  body  support  period  *  ' 

*tx,‘ty,*tz.  /*  body  translation  distance  (Earth  coord)  */ 

‘azimuth, ‘elev, ‘roll,  /*  body  Euler  angles  *, 

‘ordered  vel _mag,  /*  ordered  cockpit  velocity  (magnitude)  */ 

‘ordered  vel  dir;  /*  ordered  cockpit  velocity  (direction)  */ 


/*  driver.c  "/ 


Angle  alpha  7,.  '*  thigh  angle  */ 

gamma  7  ,  shin  angle  */ 

theta[7  :  /*  leg  lateral  angle  */ 

{ 

Device  dummy,  x,y; 

static  int  buttonflag  =  UP, pick, potentialpick,mainmenupick, submenu, slidebar; 

ini  i; 

float  barvalue; 

static  float  time; 

char  strorx  100),str_ory|100!,str_ori[l00|, 

str  trx|100],str_tryjl00j,str_rrx  100], str  timejlOOj; 


pushmatrix(); 

pushviewport(); 

viewport  (0.500,0.767); 
ortho2(0.0, 500.0.0.0.767.0); 

color(CYAN):  screen  background  color  */ 

clrar(); 

/*  Display  simulation  time  on  top  of  screen  */ 

color(TEXTCOLOR); 

time  +=  DELTA  TIME; 

sprintf(str_time, "simulation  time  %8.3f",time); 

cmov2i(  105.700); 

charstr(str  time); 

if  (qtestQ  ==  MIDDLEMOUSE)  /*  Button  just  pressed  or  released  */ 

< 

qread(&dummy): 

qread(itx); 

qread(lty); 

if  (buttonflag  ==  DOWN)  /*  Button  was  just  released  */ 

{ 

buttonflag  =  UP; 

if  (potentialpick  ==  0) 

{ 

/*  No  change  •/ 

} 

I 

I 
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driver. c 


else  if  (potentialpick  <  7)  /*  Main  menu  chosen  ' 

{ 

mainmenupick  =  potentialpick; 
pick  =  0; 

pick  =  potentialpick; 
potentialpick  =  0; 

} 

else  /*  submenu  chosen  */ 

{ 

pick  =  potentialpick;  /*  no  change  to  main  menu  pick  */ 
potentialpick  =  0; 

} 

) 

else  /*  Button  was  just  pressed.  */ 

{ 

buttonflag  =  DOWN; 

) 

}  ,*  end  of  qtest  */ 

if  (buttonflag  -  -  DOWN)  /*  Find  the  box  over  which  the 
cursor  lies  for  highlighting.  */ 

< 

x  -  getvaluator(MOUSEX); 
y  =  getvaluator(MOUSEY); 

potentialpick  =  0; 

for  (i=  l:i<7;i  +  +  )  /*  Check  the  main  menu.  */ 

{ 

if  (x  <  boxii]. right  Lb  x  >  box[i  .left 
y  <  boxlij.top  LL  y  >  boxjij. bottom) 

{ 

potentialpick  =  i; 

} 

} 

if  (submenu  ==  1)  /*  Check  submenu  #1.  * / 

{ 

for  (i=7;i<10:i-+) 

{ 

if  (x  <  boxjij. right  LL  x  >  boxjij. left  LL 
y  ^  boxlij.top  LL  y  >  boxjij. bottom) 

{ 

potentialpick  =  i; 

} 

} 

} 


driver. c 


if  (submenu  -=  2)  /*  Check  submenu  #1.  V 
{ 

for  (i  -  1 0;i<  lS;i+  +  ) 

{ 

if  (x  <  boxjii. right  kk  x  >  boxiij.ieft  kk 
y  <  boxjij.top  kk  y  >  boxiij. bottom) 

{ 

potentialpick  =  i; 

} 

} 

} 

} 

else  /*  button  is  up  */ 

{ 

potentialpick  =  0: 

} 

!*  Display  the  menu.  *  j 

for  (i=  l;i<7;i-^--«-) 

{ 

if  (i  ==  potentialpick) 

{ 

color(ACTIVEHlGHLIGHT); 

} 

else  if  (i  ---=  mainmenupick) 
color)  IN  ACTIVEHIG  HLIG  HT) ; 

} 

else 

< 

color(NOHIGHLIGHT); 

} 

rectfi(boxjij.left,  box[i|. bottom,  box(i|. right,  box|i].top); 
color(TEXTCOLOR); 

recti(box|i).left,  box ji]. bottom,  box'i]. right,  boxjij.top); 

cmov2i(box|ij.x0,boxjij.y0); 

charstr(box[i].textO); 

cmov2i(box[ij.xl,box|i|.yl); 

charstr(box[ij.textl); 

cmov2i(box|ij.x2,box|i|.y2); 

charstr(box|ij.text2); 

) 
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driver.c  ’  ' 

if  (submenu  ==  1)  /*  Display  submenu  #1.  */ 

{ 

for  (i  =  7;i<10;i-t--^) 

{ 

if  (i  ■=  —  potentialpick) 

{ 

color(ACTIVEHIGHLIGHT), 

} 

else  if  (i  ==  pick) 

{ 

color(INACTIVEHlGHLIGHT); 

} 

else 

{ 

color(NOHlGHLlGHT); 

> 

rectfi(boxfi  .left,  box jil. bottom,  boxli  -right,  boxjij.top); 
color(TEXTCOLOR); 

recti(box  ii.left.  boxli]. bottom,  box[i;. right,  boxjij.top); 

cmov2i(box|i  xO,box[ij.yO); 

charstr(box(ij.textO); 

cmov2i(boxli;.xl,box|i].yl); 

charstr(box[i{.textl); 

cmov2i(box[i  .x2,box[ij.y2); 

charstr(box(i).text2); 

) 

color(WHITE);  /*  Draw  LED  gages.  */ 

rectfi(200, 70,400, 370); 

color(BLACK); 

recti(200,70,300,150); 

recti(300,70,400,150); 

recti  (200, 150.300,230); 

recti(300, 150, 400,230); 

recti(200,280,300,310); 

recti(300, 230.400, 310); 

recti(200, 310, 300,370); 

recti(300, 310, 400,370); 

color(RED); 

cmov2i(205.350); 

ch  arstr(  "ORDERED" ) ; 

cmov2i(205,330); 

charstr("RATE"); 

cmov2i(305.350); 

charstr("  ACTUAL"); 

cmov2i(305.330); 

charstr("RATE"); 
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driver.c  *  / 


Display  the  parameter  values.  */ 
sprintf(str  orx,"%7.2n\ordered_rate-  >x); 
spr in tf ( st r  ory ,"%7.2P .ordered  rate- > y ) ; 
sprin tf( str  on . n%l .2P  .ordered  rate-  > z ) ; 
sprintf(str_trx,"%7.2f”,trans_rate->x); 
sprin t f ( str  try .  "%7 . 2P* , trans  rate-  > y ) ; 
sprintf(str_m,"3$7.2P 1  ,rot  rate- > z ) ; 

cmov2i(205.270); 
charstr(str  orx); 
cmov2i(205,190); 
c h arstr (str  ory ) ; 
cmov2i(205,110); 
charstr(str  on); 
cmov2i(305,270): 
charstr(strtrx); 
cmov2i(305.190); 
charstr(str  try); 
cmov2i(30">.110); 
charstr(str  rrz); 


} 

if  (submenu  =  =  2)  /*  Display  submenu  #2.  */ 

{ 

for  (i=10;i<13:i-r~) 

{ 

if  (i  =  =  potentialpick) 

{ 

color(ACTIVEHlGHLIGHT); 

} 

else  if  (i  ==  pick) 

{ 

color  ( IN  ACTI VEHIGHL1G  HT ) ; 

} 

else 

{ 

color(NOHlGHLIGHT); 

} 

rectfi(boxlil.left,  box(il. bottom,  box  i]. right,  box  i  . lop): 
color(TEXTCOLOR); 

recti(box|i].left,  box(ij. bottom,  box! i  right,  box  ij.top); 

cmov2i(box[ij.x0.box|i].y0); 

charstr(boxji).textO); 

emov2i(box[ij.xl.box[ij.y  1): 

charstr(boxjij.textl); 

cmov2i(boxii|.x2.box[i].y2); 

charstr(box;ij.text2); 

} 
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driver.c 


color(WHlTE);  /’  Draw  LED  gages.  */ 

rectfi(200,70,400,370); 

color(BLACK); 

recti(200, 70.300, 150); 

recti(300. 70.400. 150); 

recti(200. 150.300,230); 

recti(300. 150.400,230); 

recti(200. 230.300, 310); 

recti(300.230.400.310); 

recti(200.310.300,370); 

recti(300.S10.400,370); 

color(RED): 

cmov2i(205,350); 

charstr("ORDERED"); 

cmov2i(  205,330); 

charstr("ANGLE"): 

cmov2i(305,350); 

charstr("  ACTUAL"); 

cmov2i(305,330): 

charstr("  ANGLE"): 

> 

'*  Action!  */ 

switch  (pick) 

{ 

case  1:  submenu  =  1; 

‘selected  gait  =  FWD  WAVE  GAIT; 
break; 

case  2;  submenu  =  2; 
break; 

case  3:  submenu  =  3; 

‘program  status  =  RESET; 

break:  I 

case  4:  submenu  =  4; 

joystick(trans_rate,rot_rate, ordered  vel  mag. ordered  vel  dir.Abuttonflag); 
steering  _conv(ordered  rate, ordered  vel  mag. ordered  vel  dir, 
azimuth, tx,ty,fh); 

‘selected  gait  =  FTL_GA!T; 
break; 
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j 


driver. c  ' 


rase  5:  submenu  -  5; 

status  report(ordered  rate.trans  rate, rot  rale, 
b  footpos. period. alpha, gamma. theta, 
slow  flag, roll.elev.aiimuth.tx.ty. 
t*); 

break; 

case  6:  /*  exit  */ 

‘program  status  =  HALT; 
break; 

case  7:  bar(-200.0,200.0,&slidebar,&barvalue,trans_rate->x): 

if  (slidebar  ==  IN) 

ordered  rate- >x  -  barvalue; 

} 

break; 

case  8:  bar(-100.0,100.0,&slidebar.&barvalue,trans_rate->y); 

if  (slidebar  ==  IN) 

{ 

ordered  rate->v  =  barvalue; 

} 

break; 

case  9.  bar(- J  O,  JO,  Aslidebar.Ar  barvalue, rot  rate->i); 
if  (slidebar  ==  IN) 

< 

ordered  rate- >*  -  barvalue; 

} 

break; 

case  10:  /*  Future  expansion  */ 

break; 

case  11:  /*  Future  expansion  */ 

break; 

case  12:  '*  Future  expansion  */ 

break; 

default:  color(BLACK): 

} 

pop  view  port)); 
popmatrix(); 

/*  end  of  driver  command  *  i 


,  *  driver. c 


bar(minval,  maxval, slidebar. barvalue. act ualvalue) 
float  minvai.  maxval. ‘barvalue. act  ualvalue: 
int  'slidebar: 

{ 

register  i; 
char  str  20;; 
int  x,y; 
static  int  barlevel; 

/*  Draw  the  sliding  bar.  */ 

cursoff(); 

color(BLACK); 

rectfi(9, 69,90,690); 

color  (RED); 

recti(  10,70.30,670); 

for  ( i— 0;i  5;i - ) 

{ 

move2i(30.70  -  i*150): 
draw2i(40.70  *  i*  150); 
cmov2i(34.73  -  i*150). 

sprintf(str,  "9o6.  If, minvai  -  i*(maxvaI-minval)/4.0); 
charstr(str); 

} 

curson(); 

/*  Check  the  location  of  the  cursor.  If  it  is  inside  the 
sliding  bar,  then  calculate  the  value  for  its  position.  */ 
x  =  getvaluator(MOUSEX); 
y  =  getvaluatorf  MOUSEY); 
if  (10  <  x  tcSi  x  <  30  bit  70  <  y  &&  y  <  670) 

{ 

barlevel  -  y; 

‘slidebar  -  IN; 

‘barvalue  =  minvai  +  (maxval  -  minval)*(y  -  70)/600.0; 

> 

else 

{ 

‘slidebar  =  OUT; 

} 

/*  Draw  the  bar  showing  the  actual  level.  */ 
color(RED); 

rectf(  15. 0, 70. 0,25. 0,(370. 0^  600. 0‘actualvalue/(maxval-minval))); 

/*  Draw  the  bar  showing  the  ordered  value.  */ 
coior(  YELLOW); 
recti(  1 1 .70, 29. barlevel); 

}  /*  end  of  bar  *.' 
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'*  driver. c  * 

/*+* **** **+*********+***M****+* *********************************************  > 

joystick  (trans  rale. rot  rate. ordered  vel  mag, ordered  vel  dir, buttonflag) 

vector  *trans  rate.  *  translation  rates  of  the  center  of  gravity  in  body  coordinates  */ 
‘rot  rate;  j*  body  Euler  angle  rotation  rates  */ 

float  *ordered_vel  mag,  /*  ordered  velocity  of  cockpit  (magnitude)  */ 

‘ordered  _vel_dir;  /*  ordered  velocity  of  cockpit  (direction)  */ 

int  ‘buttonflag;  /*  indicator  for  middle  mouse  button  */ 

{ 

int  x,y,i; 

float  vx.vy,  /*  velocity  of  cockpit  in  body  coordinates  */ 

magn.dir:  /*  magnitude  and  direction  of  cockpit  velocity  vector  */ 


/*  Display  the  steering  box.  */ 

color(BLUE); 

recti(100.80,400,380); 

/*  Display  the  grid  */ 
for  (i=l;i<15;i+  +  ) 

{ 

move2(90.0-t-i*20.0,80.0); 
draw2(90.0— i*20.0, 380.0); 

} 

for  (i=l;i<15;i-^  -1-) 

{ 

move2(  100.0,80.0+ i*20.0); 
draw2(400.0,80.0+i*20.0); 

} 

/*  Display  instructions.  */ 
cmov2i(110,65): 

charstr("Hold  the  middle  button  down"); 
cmov2i(l  10,50); 

charstr("to  control  the  joystick"); 

/*  Display  the  current  velocity  of  the  cockpit.  */ 
vx  =  trans_rate->x; 

vy  =  rot  rate->*  *  HALFLENGTH  +  trans  rate->y; 
magn  =  sqrt(vx*vx  +  vy*vy); 
dir  =  atan2(vy,vx); 
if  (vx  ==  0.0) 

{ 

dir  =  0.0; 

} 

linewidt  h(5) : 
color(  YELLOW); 
move2(  250. 0,80.0); 
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/*  driver.c  */ 

if  (vx  ==  0.0) 

{ 

dir  =  0.0; 

} 

linewidth(5): 
color(  YELLOW); 
move2(250.0,80.0); 

draw2(  (250. 0-400. 0*dir),(80.0-t-magn  *3.0)); 

/*  Check  the  location  of  the  cursor.  */ 
x  =  getvaluator(MOUSEX); 
y  -  getvaluator(MOUSEY); 
if  (‘buttonflag  ==  DOWN) 

{ 

if  (100  <  x  kk  x  <  400  kL  80  <  y  LL  y  <  380) 

{ 

‘ordered  vel  mag  =  (y-80) /3.0; 

‘ordered  vel  dir  =  (250-x)/ 400.0; 

} 

} 

*  Display  the  ordered  velocity  of  the  cockpit.  */ 
linewidth(S): 
rolor(RED); 
move2(250.0,80.0); 

draw2((250.0  -  400  0  *  ‘ordered  vel  dir), (80.0  +  ‘ordered  vel  mag  *  3  0)); 
Jinewidth(l). 

}  '*  end  of  joystick  *, 


This  is  a  function  for  the  iris2400  program  walk.c. 
steering.c 


Relle  Lyman  26  Mar  1987 


#include  "gl.h" 

^include  "device. h" 

^include  "walk.h" 

#include  <stdio.h> 

^include  <math.h> 

steering  conv(ordered _rate, ordered _vel_mag. ordered  vel  dir, azimuth, tx,ty.fh) 

*  This  function  calculates  converts  ordered  head  velocity  to 
ordered  body  translation  and  rotation  rates.  */ 

float  "ordered  vel  mag,  >  *  ordered  velocity  of  the  cockpit  (magnitude)  */ 
"ordered  vel  dir.  '*  ordered  velocity  of  the  cockpit  (direction)  */ 

"azimuth.  *  body  azimuth  angle  (radians)  * 

*tx.  "ty:  *  current  position  of  the  body's  center  of 

gravity  (in  earth  coordinates)  *  ' 

vector  "ordered  rate.  *  ordered  forward  and  lateral  translation 

rates  and  azimuth  angle  rate  */ 

fh  7,;  *  selected  foothold  (in  earth  coordinates)  */ 

{ 

float  hx,hy,  -*  current  head  (cockpit)  position  (earth  coord.)*/ 

dhx.dhy,  /"  desired  head  position  (earth  coord.)  */ 

nicen_x,fticen  y,  /*  foothold  centroid  (earth  coord.)  */ 
dcgx.dcgy,  /*  desired  center  of  gravity  (earth  coord.)  */ 
dazimuth,  /*  desired  azimuth  angle  (earth  coord.)  */ 
diffazm;  /*  difference  between  desired  and  current  azimuth  '/ 

vector  desired  rate;  /*  desired  earth  translation  rates  and  azimuth 
angle  rate  */ 


*  steeringconv  */ 

/*  Note:  This  module  uses  a  level  terrain  assumption.  * ; 

/*  Calculate  current  head  position  (earth  coordinates).  */ 
hx  =  *tx  -f-  HALFLENGTH  *  cos(  'azimuth); 
hy  =  *ty  -*■  HALFLENGTH  *  sin(  'azimuth); 

/*  Calculate  the  desired  head  position  (earth  coord.).  */ 

dhx  =  hx  +  DELTA  TIME  *  'orderedvelmag  *  cos(  'ordered _vel  dir  'azimuth): 
dhy  =  hy  +  DELTA_TIME  *  'ordered  _vel_mag  *  sin(  'ordered  vel  dir  -  'azimuth): 

/*  Calculate  the  foothold  centroid.  (Forward  gaits  only)  */ 
fhcenx  =  (fhj3].x+fhl4).x+fhi5).x-t-fh[6|.x)/4.0; 
fhceny  =  (fhi3|.y+fh;4].y+fhj5|.y+fhj6].y)/4.0; 

/*  Calculate  the  desired  azimuth  angle.  '/ 
dazimuth  =  atan2((dhy-fhcen_y),(dhx-fhcen  jc)); 
diffazm  =  dazimuth  •  'azimuth: 

/*  Adjust  the  difference  to  a  value  between  pi  and  -pi.  */ 
if  (diffazm  <  -3.14159) 

{ 

diffazm  +=  6.2831853; 

} 

if  (diffazm  >  3.14159) 

{ 

diffazm  -=  6.2831853: 

) 

/*  Calculate  the  desired  center  of  gravity.  * 1 
dcgx  =  dhx  -  HALFLENGTH  *  cos(dazimuth); 
dcgy  =  dhy  -  HALFLENGTH  '  sin(dazimuth); 

/*  Calculate  the  desired  rates  (earth  coord.).  */ 
desired  rate. x  =  (dcgx  -  *tx)/DELTA_TIME; 
desired  rate.y  —  (dcgy  -  *ty)/DELTA  TIME; 
desired  rate. z  =  diffazm /DELTA  TIME; 

/*  Convert  to  body  translation  and  rotation  rates.  */ 
ordered  rate->x  -  cos(  'azimuth)  *  desired _rate.x 

-  sin(  'azimuth)  *  desired  rate. y; 
ordered  rate- >y  =  cos(  'azimuth)  *  desired  rate. y 

-  sin (  'azimuth)  '  desired  rate. x; 
ordered  rate- >z  =  desired  rate.z; 

}  *  end  of  steering  con v  */ 
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I 


This  is  a  routine  for  the  iris-2400  program  walk.r. 


This  routine  creates  a  status  report  to  be  displayed 
on  the  viewing  screen  beneath  the  ASV. 

Relle  Lyman  27  Mar  1987 


^include  "gl.h" 

4 include  "device. h" 
^include  "walk.h" 


status  report  (ordered  rate.trans  rate, rot  rate.b  foot  pos.period.alpha, gamma. 

theta, slow  flag, roll, elev, azimuth, tx,ty,tz) 
int  “slow  flag;  flag  indicating  deceleration  is  needed  */ 


Angle  theta;7  . 
alpha  7], 
gamma]7’; 


leg  component  angles  * 


float  “period,  /*  period  of  leg  cycle  ’  ; 

*tx,“ty,*tz,  /*  position  of  body  in  earth  coordinates 
“roll. “elev, “azimuth;  /*  body  Euler  angles  * 


vector  rot  rate. 


body  rotation  rates  * 


“trans  rate,  /*  body  translation  rates  */ 

“ordered  rate.  /*  ordered  lateral  and  longitudinal  and  yaw  rates 
b  footpos  7  ;  /*  foot  position  in  body  coordinates  * 

int  i.k: 

char  sir  fpx  7 ;100 |,str _fpy|7)|  100] ,str_fpz : 7 j}  100] , 
sir  orx  100  ,str_ory[100:,str_orzil06j, 
str  trx  lOOj.str  tryjlOOj.str  trzjlOOi, 
str_rrx!  100],str_rryjl00j,str  jrrzjiooj, 
str_alpha;7j  100],str_gamma[7ijl00],str  theta  7|  1001 , 
str  period 1 100], str_slowj  100., 
str_tx|  100],str_ty[  100],  strtzj  100], 
str  roll  1100  i, str  azimuth]  100; , str  elevj  100  : 


status,  c 


* 


sprint f(st  r  orx. "%  7 . 2P' , ordered  rate-  >  x ) ; 

sprint f(str  ory . ,,c^ 7 . 2f" ,ord ered  rate- >y ) ; 

sprintf(str_orz ,"%7  2F' .ordered  rate-  > z ) ; 

sprin  tf(  str  _trx.  "%7. 2F' ,  transrate-  >  x ) ; 

sprintf(str_try,"%7.2F',trans_rate->y); 

sprintf(str_trz,"%7.2P'.trans_rate->z); 

sprin tf (str _rrx,"%7. 2F' , rot  rate- > x ) ; 

sprin tf ( str  rry ,"%7.2P',rot  rate- >y ) ; 

sprin  tf  (str  rrz ,"  %7 . 2F'  .rot  rate-  >  z ) ; 

sprintf(  str  period ,  "%9.5f" ,  *  period ) ; 

sprint  f(str_tx."%7.2F',*tx); 

sprintf(str_ty."%7.2F\*ty); 

sprintf(str_tz,"%7.2F',*tz); 

sprintf(str_roll,"%7d",((int)  (‘roll  *  573.0))); 

sprintfjstr  azimuth, "%7d",((int)  (‘azimuth  *  573.0))); 

sprintfjstr  elev."9£7d",((int)  (*elev  *  573.0))); 

for  (k=l;k<7;k-*--r) 

{ 

sprintffstr  fpx  k  ,"%7.2P',b  footposikj.x); 
sprintf(str  fpy|k!."%7.2P,,b  footposikj.y); 
sprin  tf(str_fpzjkl,"%7.2P'.b_footposjkl.z): 
sprintf(str  alpha[ki,"%7d",alphajkj); 
sprintf(str_gamma|k', "%7d", gamma  ki): 
sprintf(str  theta[k|,"%7d",thetajkj); 

} 

pushmatrix(); 
viewport  (0.400,0, 767); 
ortho2(0.0, 400.0, 0.0, 767.0); 
color(BLACK); 
rectfi(10, 10, 400.370); 
color(YELLOW); 
rectfi(20.20.390,360); 
color(BLACK); 
cmov2i(220,340). 
charstr("X"); 
cmov2i(280,340); 
charstr("Y"); 
cmov2i(340.340); 
charstr("Z"); 

cmov2i(30,325): 
c  harstrf  "ordered  rate" ) ; 
cmov2i(30,310): 
charstr("trans_rate"); 
cmov2i(30,295); 
charstr("rot  rate"); 
cmov2i(30,280): 
charstr("position"): 
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status. c  / 


cmov2i(2 10,265): 
charstrf'ROLL") ; 
cmov2i(260,265); 
charstr("ELEV."); 
cmov2i(310,265); 
charstr( "  A  ZIMUTH" ) ; 

cmov2i(  30,250); 
charstrf'current  attitude"); 
cmov2i(30,235); 
charstr(  "ordered  attitude"); 

cmov2i(30,210); 
charstrf  "period") ; 

iff *slow  flag  ==  SLOW)  /*  moving  too  fast  */ 

{ 

cmov2i(  750,220); 
color  (RED); 
charstrf'TOO  FAST"). 
color(BLACK); 

} 

cmov2i(30,185); 

charstrf'x  ft  pos  (1-3)"); 

cmov2i(l  10.170): 

charstr("(4-6)"); 

cmov2i(30,155); 

charstr("y  ft  pos  (1-3)"); 

cmov2i(U0,140); 

charstr("(4-6)"); 

cmov2i(30,125); 

charstr("z  ft  pos  (1-3)"); 

cmov2i(U0,U0); 

charstr("(4-6)"); 

cmov2i(S0,95); 

charstr("ALPHA  (1-3)"); 

cmov2i(110,80); 

charstr("(4-6)"); 

cmov2i(3O,05); 

charstr(  "GAMMA  (1-3)"): 

cmov2i(  1 10.50); 

charstr("(4-6)"): 

cmov2i(30.35); 

charstrf  "THETA  (1-3)"); 

cmov2i(  110.20); 

charstr("(4-6)"): 
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cmov2i(  180.325); 

charstr(strorx); 

cmov2i(240,325); 

chamr(strory); 

croov2i(500,325); 

charstr(strorz): 

cmov2i(  180.310): 
charstr(str  trx)' 
cmov2i(240.310): 
charstr(str  try ): 
rmov2i(  300,3 10); 
charstr(str  trz); 

cmov2i(  180,295): 
charstr(str  rrx): 
cmov2i(240,295): 
charstr(str  rry); 
cmov2i|300,295): 
charstr(str  rrz): 

cmov2i(  180,280): 
charstrfstr  tx); 
cmov2i(240,280): 
charstrfstr  ty); 
cmov2i(300.280); 
ch*rstr(str  t*); 

cmov2i(  180,2 10), 
charstr(str  period ); 

cmov2i(  180,250): 
charstrfstr  roll); 
cmov2i(240,250): 
charstr(str  elev): 
cmov2i(300.250): 
charstr(str  azimuth); 


*  status. c  ” ' 

for  (i=l;i<4;i  — (-) 

{ 

k-  1 10*i*70; 
cmov2i(k,185); 
charstr(str  fpxjij); 
cmov2i(k,170); 
charstr(str_fpx[i+3]); 
cmov2i(k,155); 
charstr(str_fpy!ij); 
cmov2i(k,140); 
charstr(str  _fpy[i-f-3|); 
cmov2i(k,125); 
charstr(str  fpzjij); 
cmov2i(k,110); 
charstr(str_fpz|i^  3]); 
cmov2i(k,95); 
charstr(str  alpha  i  ); 
cmov2i(k,80); 
chamr(stralpha  i  +  3j): 
cmov2i(k,65): 
charstr(strgamma|i  ); 
cmov2i(k,50); 
charstr(str  gammaji  — 3] ); 
cmov2i(k,35); 
charstr(st r  t  heta(i| ) ; 
rmov2i(k.20); 
charstr(str  theta(i+  3  ); 

} 


popmatrix(); 


This  is  a  function  for  the  iris  2400  program  walk.c. 
support,  c 


Relle  Lyman  21  Aug  1986 

**********************»**********»****«*%**#******************»*** y 

4  include  "gl.h" 

#include  "device. h" 

#include  "walk.h" 

#include  <stdio.h> 

#include  <math.h> 

support  plane(spe) 

/*  This  module  will  compute  a  new  estimated  support  plane  based  on 
the  position  of  the  supporting  legs.  As  a  temporary  measure  it 
is  assumed  the  support  plane  is  flat  and  at  "sea  level"  (i.e. 
z  =  0  ).  The  equation  for  the  plane  is  Ax-By-*-Cz-t-D=0.  */ 

plane  *spe;  estimated  support  plane  in  earth  coordinates 

{ 

spe->a  =  0.0; 
spe->b  =  0.0; 
spe->c  =  1.0; 
spe->d  —  0.0; 

} 


This  is  a  function  for  the  iris  2400  program  waik.c. 
body  rates. c 


Relle  Lyman  19  Apr  1987 

•**•»••****•*•*••»*•*»*••**>•***  «•*«*****»***»*»  •■XlMXMtXtOMi  f 

# include  "gl.h" 

•  include  "device. h" 

•  include  "walk.h" 

•  include  <stdio.h> 

•  include  <math.h> 

body  rates(rot  rate. trans  rate. spe.h, in vh, ordered  rate, tx,ty,tz, 
roll,  elev,  azimuth) 

/*  This  function  computes  the  body  rotation  and  translation  rates.  */' 

vector  *rot  rate,  /*  rotation  rate  * / 

‘trans  rate,  /*  translation  rate  */ 

‘ordered  rate:  /*  ordered  x  translation,  y  translation, 
and  z  rotation  rates  *  / 

plane  *spe;  /*  support  plane  in  earth  coord  */ 

float  h [4 j [4],  /*  homogeneous  transformation  matrix  */ 

invh{4);4|,  /*  inverse  transformation  matrix  */ 

*tx,*ty,*tz,  /*  position  of  body  in  earth  coordinates  * 

‘roll, ‘elev, ‘azimuth;  /*  body  Euler  angles  */ 

{ 

int  ij; 

float  eta,  .'*  body  plane  attitude  wrt  earth  plane  */ 
eta  d,  /  *  desired  body  plane  attitude  *  / 
height,  /*  distance  form  CG  to  est.  support  plane  */ 
height  d,  /*  desired  height  */ 

gamma,  /*  angle  between  desired  and  present  body  unit  normal  vectors  * 

kx,  /*  x  component  of  rotation  unit  vector  in  body  coord  */ 

ky,  /*  y  component  of  rotation  unit  vector  in  body  coord  */ 
ka,  /*  control  law  gain  */ 

a,b,c,  /*  body  plane  desired  unit  normal  in  body  coord  */ 
length,  /*  rotation  vector  normalizing  factor  */ 
celev,selev,telev,  /*  sine. cosine, tangent  of  elev  */ 
croil,sroll,cazim,sazim,  /*  sin  and  cos  of  roll  and  azimuth  *  ' 
m; 

plane  spb:  /*  support  plane  in  body  coordinates  */ 

vector  db_unit_norm,  /*  desired  body  plane  unit  normal  in  earth  coordinates  */ 
trans  rate  e,  !*  Translation  rates  in  earth  coordinates  *, 
rot  rate  e;  /*  Rotation  in  body  Euler  rates  *< 
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/*  body  rates. c 


/*  Calculate  the  body  plane  attitude,  (level  ground  assumption!)* 
eta  =  0.0; 

/*  Calculate  desired  body  plane  attitude  (level  ground  assumption!)*/ 
eta  d  =  eta; 

/*  Calculate  the  desired  body  clearance,  (level  ground  assumption!)*  ' 
height  d  =  HO; 

/*  Calculate  the  support  plane  coefficients  in  body  coordinates.  * , 

/*  |spb|  T  =  jspej  T  *  h  *, 

spb.a  =  spe->a  *  hj0j(0|  spe->b  *  hi  I  j|0  +  spe->c  *  h;2  |0  +  spe->d  *  h  3/0i 

spb.b  =  spe->a  *  h[0j[l]  *  spe->b  *  h 1 1 ; [  1 ;  +  spe->c  *  h  2  |l;  +  spe->d  *  h  3  lj 

spb.c  =  spe->a  *  hj0][2|  -  spe->b  *  h[  1] :2;  -  spe->c  *  h|2][2  spe->d  *  h|3H2' 

spb.d  =  spe->a  *  h|0'|3l  -  spe->b  *  hjl!(3  +  Spe->c  *  h  2  !3  +  spe->d  *  h  3  3 

/*  Height  of  body  CG  above  support  plane  */ 
height  =  spb.d; 

Calculate  desired  unit  normal  for  the  body  plane  in  earth  coord.  *./ 
m  =  sqrt(spe->a  *  spe-^a  +  spe->b  *  spe->b); 

•'*  check  for  division  by  zero  */ 
if  (m>0.0) 

< 

db  unit  norm. x  =  spe->a  *  sin(eta  d)  ,  m; 

db  unit  norm.y  =  spe->b  *  sinfeta  d)  /  m; 

} 

else 

{ 

db  unit  norm.x  =  0.0; 

db  unit  norm.v  =  0.0; 

} 

db_unit_norm.z  =  cos(eta  d); 


body  rates. c  */ 


/*  Transform  the  desired  unit  norma!  vector  of  the  body  plane 
from  earth  to  body  coordinates.  !a,b,c|T=:invhr,'b_unit_norm  * 

*  Note:  invhr  is  the  inverse  of  the  rotational  transformation 
submatrix  (first  three  rows  and  columns  of  h).  */ 

a  =  invhj0:[0]*db_unit  norm.x  +  invhjO]jl]*db  unit  norm. y  + 
invh  0  2i*db_unit_norm.z; 

b  =  invh  l  jO  *db_unit  norm.x  +  invh[lj[l j’db  unit  norm.y  * 
invh  1  2|*db_unit_norm.z; 

c  =  invhi2  O|’db_unit_norm.x  +  invh;2][l]*db_unit_norm.y 
invh  2  [2)*db_unit_norm.z; 

,  *  Control  law  yielding  an  exponential  response  */ 

,  *  d  gamma/dt  =  -ka  *  gamma  */ 

ka  =  1/TIME  CONSTANT  1; 

gamma  =  acos(c); 

length  =  sqrt(a*a  +  b*b); 

if  (length  <  .00001) 

{ 

kx  =  0: 
ky  ^  0: 

} 

else 

{ 

•'*  components  of  rotation  unit  vector  in  body  coordinates  */ 
kx  =  -b  length; 
ky  =  a /length; 

} 

i*  Calculate  rotation  and  translation  rates  *  ‘ 
trans  rate- >z  =  -ka  *  (height  d  -  height): 
rot_rate->x  =  -ka  *  kx  *  gamma; 
rot  rate- >y  =  -ka  *  ky  *  gamma; 

*  Rate  =  dt  *  acceleration  +  old  rate  *  / 

trans_rate->x  =  DELTA_TIME  *  (ordered_rate->x  -  trans  rate-  >x)/ 
TIME  CONSTANT  2  +  translate- >x; 
trans  rate- >y  =  DELTA  TIME  *  (ordered  rate- >y  -  translate- >y)/ 
TIME  CONSTANT  3  -  trans  rate- >y; 
rot  rate->z  =  DELTA  TIME  *  (ordered  rate- >z  -  rot_rate->z)  / 
TIME  CONSTANT  3  -r  rot  rate- >z; 

*  Conversion  to  Earth  coordinate  translation  rates.  */ 

croll  -  cos(*roll); 
sroll  -  sin ( *roll) ; 
telev  =  tan(*elev); 
celev  =  cos(*elev); 
selev  =  sin(*elev); 
cazim  =  cos(*azimuth); 
sazim  =  sin  ( ‘azimuth); 
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*  body  rates. c  */ 

trans  rate  e.x  =  trans_rate->x  *  croll*cazim  -+- 

trans_rate->y  *  (cazim*sroll‘selev  -  sazim*celev)  - 
trans_rate->z  *  (sazim*selev  -  cazim*sroll*celev): 
trans_rate_e.y  =  trans_rate->x  *  croll'sazim  + 

trans_rate->y  *  (sazim*sroll*selev  —  cazim*celev)  + 
trans_rate->z  *  (cazim*selev  -  sazim*sroll*celev); 
transratee.z  =  -trans_rate->x  *  sroll  + 
trans_rate->y  *  croll*selev  - 
trans_rate->z  *  cazim*celev; 

/*  Conversion  to  body  Euler  rates  *  / 

rotratee.x  =  rot_rate->x  *  rot_rate->y  *  telev  *  sroll  + 
rot  rate- >z  *  telev  *  croll; 

rot  ratee.y  =  rot  rate->y  *  croll  -  rot_rate->z  *  sroll; 
rot  rate  e.z  =  rot  rate->y  *  sroll  /  celev  + 
rot  rate->z  *  croll  /'  celev; 

!*  Integration  routine  *  j 

*tx  —  =  trans  rate  e.x  *  DELTA  TIME: 

*ty  -  -  trans  rate  e.y  *  DELTA  TIME: 

*tz  +=  trans  rate  e.z  *  DELTA  TIME; 

*roll  —  =  rot  rate  e.x  *  DELTA  _TIME: 

*elev  ■+-  rot  rate  e.y  *  DELTA  TIME; 

*  azimuth  -r=  rot  rate  e.z  *  DELTA  TIME; 

■'*  Update  the  H  matrix  * / 

croll  =  cos(*roll); 
sroll  =  sin(*roll); 
telev  =  tan(*elev); 
celev  =  cos(*elev); 
selev  =  sin(*elev); 
cazim  =  cos(*azimuth); 
sazim  =  sin(*azimuth); 
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/*  body  rat  es  c  *  / 

h  [0  j  [Oj  =  croll*cazim; 

hjOjjlj  =  cazim*sroll*selev  -  sazim’celev; 

h[0)|2j  =  sazim'selev  +  cazim*sroll*celev; 

hjOjlSj  =  *tx; 

h[l||0]  =  sazim'croll; 

h }  1  j  1 1  j  =  cazim'celev  •+  sazim*sroll*selev; 

h|l]|2]  =  sazim'sroll*celev  cazim*selev; 

h  j  1  j  [3  j  =  *ty: 

h [2  j  [Oj  =  -sroll; 

h  [  2  j  [  1  j  =  croll*selev; 

h(2j|2j  =  croll*celev; 

h  ( 2  j  [  3  j  =  *tz: 

h|3j|0|  =  0.0; 

h!S||l]  =0.0; 

hl3)(2l  =  0.0; 

h[3]|3j  =  1.0; 

/*  inverse  homogeneous  transform  matrix  */ 

for  (i=0;  i<3;  i - ) 

{ 

for  { j =0;  j<3;  j-r+) 

{ 

invh  i  [ji  =  hijjjil; 

} 

invh[3j|i|  =  0.0; 

invh!il;3j  =  -|hf0jji’*h!0l|3;  -  bjl](i:*hjl|[3]  -  hj2)!i;*h[2]j3:); 

invhj3j|3  =  1.0; 

}  /  *  end  of  body  rates  * , 
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/*v***S****»**x*****k*«,e**x****xk:k*** x^****,****,, ***,*,«*,,,**«**** 

This  is  a  function  for  the  iris  2400  program  walk.c. 
con  work  vol.c 


Relle  Lyman  19  Apr  1987 

**/ 


^include  "gl.h" 

# include  "device. h" 
^include  "walk.h" 
f  include  <stdio.h> 
# include  <math.h> 


con  work  vol(cwv.b_footpos,leg  status, warning) 

/*  This  module  will  compute  a  new  constrained  working  volume  for 
improved  stability  on  rough  terrain.  Currently  all  values  are 
set  for  a  perfectly  flat  support  plane.  Dimensions  are 
expressed  in  cm.  * ! 


work  vol  cwv|7  ;  /*  constrained  working  volume  in  body  coordinates  */ 
vector  b  footpos  7  ;  /*  foot  position  in  body  coordinates  */ 


int 


{ 

int 


leg_status'7j,/*  status  of  leg  (supporting,  liftoff,  transfer, 
placement)  *  ' 

’warning;  /'*  flag  indicating  supporting  leg  is  outside  of 
the  working  volume  * / 


‘warning  =  OFF; 

for  (i=l;i<7;i+  +  )  /*  check  each  leg  */ 

{ 

if  (leg  status  i  ==  SUPPORTING) 

{ 

if  ((b  footpos  ij.x  <  cwv(ij.x.min)i| 

(b  footposji  .x  >  cwv[i).x.max)j| 

(b  footposji  .y  <  cwv|i|.y.min)|| 

(b  footpos  i  .y  ">  cwv|i|.y.max)ji 

(b  footposlii.z  <;  cwv|ii.z.min); 

(b  footpos  i  .z  >  cwvii. .z.max))  /*  outside  limits  * 

{ 

‘warning  =  ON; 

} 

} 

} 
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'*  con  work  vol.c  *. 

if  (‘warning  ==  ON) 

{ 

pushmatrixQ; 

pushviewport(); 

view  port  (0,180.0, 80); 

ortho2(0.0,lS0.0, 0.0.80.0); 

color(RED); 

rectfi(  10, 10, 150,70); 

color(BLACK); 

cmov2i(  10,30); 

charstr("  OUTSIDE  CWV"); 

popviewport(); 

popmatrix(); 

} 

} 
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This  is  a  function  for  the  iris2400  program  walk.c. 
opt  period  .c 


Relle  Lyman  29  Apr  1987 

*********************************************************** 

♦  include  "gl.h" 

#include  "device. h" 
f  include  "walk.h" 

^include  <stdio.h  > 

♦  include  <math.h> 

optimal  _period ( legphase,b_footpos ,rot  rate , trans _rate ,cwv ,leg  status, period ) 

/*  This  function  computes  the  optimal  period  for  walking.  *  / 

vector  ’rot  rate.  /*  body  rotation  rate  */ 

’trans  rate.  ,'*  body  translation  rate  * , 
b  footposl7|;  '*  position  of  foot  in  body  coordinates  */ 

work  vol  cwv  7.;  /*  constrained  working  volume  ’  ' 

float  legphase[7;,  /*  phase  of  leg  *  ' 

’period;  /*  body  support  period  */ 

int  leg  status(7);  /*  status  of  leg  0  =  supporting  */ 

{ 

vector  b  footvel;  /*  foot  velocity  */ 

float  fx.fy.fit,  /*  foot  position  */ 

tmin.  /*  minimum  temporal  kinetic  margin  */ 

tx,ty,tz.  /*  temporal  kinetic  margins  */ 
d,  /*  distance  to  cwv  limit  */ 

speed,  /*  magnitude  of  body  velocity  */ 
fsperiod,  /*  forward  support  period  */ 
bs  period,  /*  backward  support  period  */ 
min  fs  period,  /*  minimum  forward  support  period  */ 
min_bs_period,  /*  minimum  backward  support  period  */ 
fs  phase,  /*  forward  support  phase  ’/ 
bs  phase.  /*  backward  support  phase  */ 
mvx.mvy.mvz; 

int  i. 

minleg; 

static  int  gait=FORWARD;  Wave  gait  direction  */ 
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mRRRmDii 


'*  optimal  period 

/*  Initialize  variables  * 
tmin  -  LONG  TIME; 
minfs  period  =  LONG  TIME: 
minbsperiod  -  LONG  TIME; 

/“  For  each  leg  compute  the  maximum  instantaneous  support  period.  */ 
for  (i=  1;  i<7;  i+  +  ) 

{ 

'*  Support  check  *  / 

if  (leg  status  ij  ==  SUPPORTING) 

{ 

/*  Compute  foot  velocity.  */ 

b_footveI.x  =  -(trans_rate->x)  —  (rot  rate->z  *  b  footpos(i).y) 

-(rot  rate->y  *  b  footpos  i  .z); 
b  footvel.y  =  -(trans_rate->y)-(rot_rate->z  *  b  footposjii.x) 

4- (rot  rate- >x  *  b  footposjij.z): 
b  footvel.z  =  -(trans  rate->z)  +  (rot  rate->v  *  b  footpos  i  x) 

-(rot  rate->x  *  b  footpos  i  .y); 

'*  Check  to  see  if  foot  is  in  cwv.  * , 
fx  =  b  footpos  i  x; 
fy  =  b  footpos  ii  v; 
fz  =  b  footpos  i  ,z: 

if  ((fx<  cwv  i  x.min)  :  (fx  cwv'i  .x.max): 

(fy *  cwv  i  y .mm)  (fy -  cwv  i  .y.m&x)  [ 

(fz*.  cwv  i  .z.min)|  (fz  >  cwv[i  .z.max))  /*  outside  cwv  */ 

{ 

tmin  =  0.1: 

} 

else 

{ 

i*  Compute  distance  to  x  limit  and  the  temporal 
kinetic  margin  in  the  x  direction.  */ 
if  (b  footvel.x  >  0.0) 

{ 

d  -  cwv  ij. x.max  -  fx; 
tx  =  d  i  b  footvel.x; 

} 

else  if  (b  footvel.x  <  0.0) 

{ 

d  =  fx  -  cwv  ij. x.min; 
tx  =  d  i  -b  footvel.x: 


} 


optimalperiod  */ 


/*  Check  for  minimum  value.  */ 
if  (tx<tmin) 

{ 

tmin  =  tx; 

} 

/*  Compute  distance  to  y  limit  and  the  temporal 
kinetic  margin  in  the  y  direction.  * / 
if  (bfootvel.y  >  0.0) 

{ 

d  =  cwv[i|.y.max  -  fy; 
ty  =  d  /  b_footvel.y; 

} 

else  if  (b  footvel.v  <  0.0) 

{ 

d  =  fy  -  cwvjij.y.min; 
ty  =  d  /  -b  footvel.y; 

} 

else 

{ 

ty  =  LONG  TIME; 

} 

/*  Check  for  minimum  value.  */ 
if  (ty<tmin) 

{ 

tmin  =  ty; 

} 


/*  Compute  distance  to  z  limit  and  the  temporal 
kinetic  margin  in  the  z  direction.  */ 
if  (b  footvel.z  >  0.0) 

{ 

d  =  cwvjij.z.max  -  fz; 
tz  =  d  /  b  footvel.z; 

} 

else  if  (b  footvel.z  <  0.0) 

{ 

d  =  fz  -  cwv(i|.z.min; 
tz  =  d  /  -b  footvel.z; 

else 

{ 

tz  =  LONG  TIME; 

} 

/*  Check  for  minimum  value.  */ 
if  (tz<tmin) 

{ 

tmin  =  tz; 

} 

}  /*  end  inside  cwv  */ 


optimal  period  */ 


/  *  Compute  the  support  phase  for  forward  and  backward  gaits.  *,• 
fs  phase  =  legphaseji  /BETA; 
bsphase  -  (BETA  *  legphaseji])/ BETA; 

/ *  Compute  support  period.  */ 
fsperiod  —  (tmin-0.1)/(1.0  -  fsphase); 
bs  period  =  (tmin-0.L)/(1.0  -  bs  phase); 

/*  Find  the  minimum  support  period.  */ 
if  (fs  period  <  min  fs  period) 

{ 

min_fs_period  =  fs_period; 

} 

if  (bs  period  <  m in  bs  period) 

{ 

min  bs_period  —  bs  period; 

} 

}  /*  end  support  check  */ 

)  '*  end  leg  loop  */ 

**  Choose  gait.  *  / 

speed  sqrt(trans_rale->x  *  trans_rate->x  + 
trans  rate->y  *  trans  rate- >y); 

if  ((speed  OUTER  LlMIT)&&(trans  rate->x  >  INNER  LIMIT)) 
gait  -  FORWARD: 

> 

else  if  ((speed<OUTER_LIMIT)J:&(trans _rate->x<  -INNER  LIMIT)) 

{ 

gait  =  BACKWARD, 

} 

else 

{ 

/*  No  gait  change.  */ 

} 

if  (gait  ==  FORWARD) 

{ 

*period  =  m  infsperiod; 

} 

else 

{ 

•period  =  min  bs_period; 

} 

}  /*  end  optimal  period  */ 
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********»********»************«********«*******»*.******»*»***«**„,* 

This  is  &  function  for  the  iris  2400  program  walk.c. 
decelerate,  c 


Relle  Lyman 


04  May  1987 


/include  "gl.h" 
#include  "device. h" 
/include  "walk.h" 
/include  <stdio.h> 
/include  <math.h> 


decelerate(rot_rate, translate, period, slow  flag, min  period) 

This  function  computes  the  foot  transfer  rate  and  slows  the 
vehicle  if  the  maximum  rate  is  exceeded.  */ 

vector  *rol  rate.  /*  body  rotation  rate  */ 

*trans  rate:  /*  body  translation  rate  */ 

float  ‘period.  /*  optimal  period  for  the  leg  cycle  */ 

*min  period:  /*  minimum  leg  period  */ 

int  ‘slow  flag:  '*  flag  indicating  vehicle  has  been  slowed.  */ 

< 

int  ij: 

float  transfer  time;  /*  time  from  liftoff  to  placement  * / 


if(*period  <  ‘min  period)  /*  slow  down  */ 

{ 

‘slow  flag  =  SLOW; 

‘period  =  ‘minperiod; 

trans  rate->x  *=  .95; 
trans  rate->y  *=  .7; 
trans  rate*>z  *=  .95; 
rot  rate- >x  *=  .95; 
rot  rate->y  *=  .95; 
rot  rate->z  *=  .7; 
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*  decelerate. c  */ 

i*  display  warning  on  screen  */ 

pushmatrixQ; 

push  viewport!); 

viewport  (200,530,0.80); 

ortho2(200.0, 330.0, 0.0, 80.0); 

color(  YELLOW); 

rectfi(  2 10, 10,330,70); 

color(BLACK); 

cmov2i(210,30); 

charstr("  DECELERATION  "); 

popviewport(); 

popmatrix(); 

} 

else 

{ 

’slow  flag  —  NORMAL; 

} 


}  •  *  end  of  decelerate*/ 
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/ 

This  is  a  function  for  the  iris2400  program  walk.c. 
legphase.c 


Relle  Lyman  24  Aug  1986 

*S*«***X*X*****V*** **************X*****»*****#***W*»******** 

#include  "gl.h" 

#inciude  "device. h" 

# include  "walk.h" 

^include  <stdio.h> 

#include  <math.h> 

leg  phase(  legphase,  rel  legphase,  period) 

/*  This  function  computes  the  phase  for  each  leg.  */ 

float  legphase|7  ,  f*  phase  of  leg  */' 

rel  legphasei7;, /*  relative  phase  of  leg  */ 

‘period;  !*  body  support  period  */ 

{ 

static  float  bodyphase  =  0.0;  /*  kinematic  phase  of  body  *  ‘ 
float  modulus_one();  /*  modulus  one  function  */ 
int  i: 


' *  Calculate  new  body  phase.  */ 

bodyphase  -  modulus  one(bodyphase  +  DELTA  TIME/ (‘period)  ): 

'*  Calculate  new  phase  for  each  leg.  (modi)  */ 
for  (i=l;  i<-7;  i-f  -) 

{ 

legphaseii  =  modulusone(bodyphase  -  rel _legphase[ij); 

} 


}  /*  end  of  leg_phase  */ 


This  is  a  function  for  the  iris2400  program  walk.c. 
ft  traj.c 


Relle  Lyman 


19  Apr  1987 


/ 


#include  "gl.h" 

♦  include  "device. h" 

#inc!ude  "walk.h" 

#  include  <stdio.h> 
finclude  <math.h> 

foot  _trajectory(legphase,period,leg_stat  us, foot  pos.b_footpos,fh, 
oldfh,invh,h,cwv, translate, rot  _rate, selected  _gait) 

/*  This  function  calculates  the  trajectory  for  each  foot.  */ 

float  legphase|7 /*  Phase  of  individual  leg  */ 

•period,  /*  Optimal  period  * j 

h [ 4 } 1 4 J ,  /*  Homogeneous  transformation  matrix  */ 

invh|4:(4  ;  *  Inverse  transformation  matrix  */ 


vector  footpos!7;, 

b_footpos[7j, 

fh  7'.  /*  Foothold  selection  (earth  coordinates)  */ 

oldfh'7],  /*  Old  foothold  selection  (earth  coordinates)  */ 
*trans  rate,  /*  Body  translation  rate  * / 

*rot  rate;  /*  Body  rotation  rate  */ 

work  vol  cwv  7|; 

int  leg  status(7i.  j*  State  of  individual  leg  * / 

‘selected  gait;  /*  Desired  tripod  gait  */ 


{ 

float  trans  time,  /*  Time  required  to  go  from  leg  liftoff  to  leg  touchdown  */ 

end  lift  phase,  j*  Point  in  transfer  phase  where  liftoff  ends  */ 
begin  place  phase,  /*  Point  in  transfer  phase  where  placement  begins  */ 
trans_phase;  /*  Leg  transfer  phase  */ 

static  int  liftoff _flag|7)=OFF,  /*  Indicates  first  time  entering  the 
subroutine  in  the  current  leg  cycle.  */ 
transfer_flag|7!=OFF, 
place  _flag’  7j  =  OFF; 

static  vector  d_footpos|7  ;  /*  Desired  foot  position  */ 

int  i;  /*  Leg  number  */ 
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*  foot  trajectory  *  j 

j*  Calculate  the  time  required  to  move  a  leg  from  liftofT  to 
touchdown.  (Transfer  time)  */ 
trans  time  =  (1.0-  BETA)  *  ABS(*period), 

/*  Calculate  phase  points  marking  change  of  transfer  mode 
(direction).  Note:  Modify  later  to  account  for  transfer 
time.  */ 

end  lift  phase  =  0.2; 
begin  place  phase  =  0.8; 

/*  For  each  leg  */ 

for  (i=l:  i<7;  i-*-  +  ) 

{ 

/*  Calculate  transfer  phase.  */ 

/*  (lift  =  0.0  place  =  1.0  )  * / 
if  (‘period  <  0) 

{ 

trans  phase  =  (1.0-  legphasej i j ) / ( 1 .0  -  BETA); 

} 

else 

{ 

trans  phase  =  (  legphaseji1  -  BETAJ^l.O-  BETA); 

} 

■  Find  the  leg  transfer  state.  *  ' 
if  (trans  phase  <  0.0) 

{ 

leg  status^  =  SUPPORTING; 

support  trajec tory (liftoff  flag, place Jlag, transfer  flag, foot pos, 
b  footpos,invh.i); 

} 

else  if  (trans  phase  <  end  lift  phase) 

{ 

leg  status(i|  =  LIFTOFF; 

lift  trajectory (liftoff  flag, place  flag, transfer  flag, footpos, 

b  footpos, invh,&trans_time,&trans  phase, &endjift_phase,i) 

} 

else  if  (trans  phase  <  begin  place  phase) 

{ 

leg  status[ij  =  TRANSFER  FORWARD; 
transfer_trajectory( liftoff  flag, place  flag, transfer  flag, footpos, 
b  footpos,h,invh,&trans  time,&trans_phase, 

L begin _place_phase,i,cwv, trans  rate, rot  rate, fh.oldfh. 
period, selected  gait); 

} 
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/*  fool  trajectory  */ 


else  ,  *  end  placephase  <  trans  phase  <.  1.0  */ 

{ 

leg  status  i  -  PLACEMENT; 

placement  trajectory (liftoff_flag,place_flag, transfer  flag, foot pos, 
b_footpos,invh,&trans  time, &trans  phase, i); 

} 

} 


}  /*  end  of  foot  trajectory  *  / 


■  ’  foot  trajectory  */ 

/xxxxxxxxxx>xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxtxxxxxxxxxx  xxxxxxxxxxxxxxxxxxxxxx  r 

/ 

lift  _trajectory(iiftoff  flag, place  flag, transfer  flag. foot  pos.b  footpos. 
invh,trans_time,trans_phase.end  lift  phase, leg  number) 

/*  This  function  calculates  the  trajectory  of  the  foot  while  it  is 
being  lifted  from  the  ground.  It  is  called  from  foot  trajectory)).*/ 

vector  footpos' 7],  /*  Present  foot  position  in  earth  coordinates  */ 

b_footposj7|;  /*  Present  foot  position  in  body  coordinates  */ 

float  *trans_time. 

*end  lift  phase, 

‘transphase, 

invh  4 j ; 4  ;  '*  Inverse  homogeneous  transformation  matrix  */ 

int  liftofT_flagj7  ,  /*  Indicates  the  first  time  entering  subroutine 
in  the  current  leg  cycle.  */ 
transfer  flag(7\ 
place  flag;7j, 
leg  number; 

{ 

float  lift  time; 
int  i: 

static  vector  d  footpos  7  ;  *  Desired  foot  position 
in  earth  coordinates  */ 

i  =  leg  number: 

/*  Calculate  the  desired  footposition.  */ 
if  (  liftoff  flagiil  !=  ON) 

{ 

dfootposji].*  =  footposjij.*  +  FOOTLIFTHE1GHT; 
liftoff  flagji!  =  ON; 
transfer  flagji]  =  OFF; 
place  flag[il  =  OFF; 

} 

*  Calculate  the  time  required  to  reach  the  desired  height 
from  the  present  foot  position.  */ 
lift  time  =  *trans  time  *  (*end  lift  phase  -  *trans  phase); 


*  foot  trajectory  *  •' 

,  *  Calculate  the  new  foot  position.  (Earth  Coordinates)  *, 
if  (DELTA  TIME  <  lift  time) 

{ 

footpos|ij.z  -=  (dfootposiij.z  -  footposij.z) 

•  DELTA  TIME  /  lift  time; 

} 

else  /*  Last  increment  of  time  *  > 

{ 

footposjij.z  =  d_footpos!ij.z; 

} 

/*  Transform  to  body  coodinates.  */ 

/*  [b  footpos jj|T  =  invh  *  (footposjijjT  */ 
transform _point(b  foot pos.invh.footpos.i); 

}  /  *  end  of  lift  trajectory  *  I 
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f'S'X'S.VS 


/*  fool  _trajectory  *  / 

/»***»**»•****»*»*»«*.»**«**«*»*»*«..»,»***»».»**»««**•».*«»*»**»*****»**,,** 

transfer  trajectory  (liftoff  flag, place  flag,  transfer  flag, footpos. 

b  footpos.h,invh.  trans  time.trans  phase. begin  place  phase, 
leg  number, cwv,trans  rate, rot _rate,fh,oldfli,period, selected  gait) 

I*  This  function  calculates  the  trajectory  of  the  foot  during  the 
phase  in  which  the  foot  is  transfered  forward.  The  function 
is  called  from  foot_trajectory().*/ 

vector  footpos  7],  /*  Present  foot  position  in  earth  coordinates  */ 

b_footpos[7j,  ,  *  Present  foot  position  in  body  coordinates  * / 
fh[7j.  /*  Selected  footholds  (earth  coordinates)  ’/ 
oldfh|7j,  /*  Old  selected  footholds  (earth  coordinates)  */ 

‘trans  rate,  /*  Body  translation  rate  */ 

’rot  rate;  /*  Body  rotation  rate  */ 

work  vol  cwv|7  :  /  *  Constrained  working  volume  in  body  coordinates  */ 

float  ’trans  time, 

'begin  place  phase, 

’trans  phase. 

’period.  /*  Optimum  period  of  gait  */ 
h  4  ‘4j.  /’  Homogeneous  transformation  matrix  */ 

invh  4j  4];  /*  Inverse  transformation  matrix  */ 

int  liftoff  flag[7j,  /*  Indicates  the  first  time  entering  subroutine 
in  current  leg  cycle  */ 
transfer  _flag[7], 
place  flag; 7 i . 

’selected  gait,  /*  Desired  tripod  gait  */ 
leg  number; 

{ 

float  trans  fwd  time,  /*  Time  remaining  in  the  transfer  forward  phase  */ 
vx.vy,  /*  Velocity  of  cockpit  in  body  coordinates  */ 

rel  hd,  /*  Relative  heading  of  cockpit  velocity  */ 

proj  dist,  /*  Projected  distance  forward  for  new  footholds  */ 
min  time;  /*  Minimum  time  to  reach  any  cwv  limit  */ 

int  i; 


t 


vector  cwv_velocity,  /*  Instantaneous  velocity  of  the  center  of 
the  cwv  (earth  coodinates)  *  ! 
time  to  limit,  /*  Time  to  reach  the  cwv  limits  */ 
bfh [ 7  ,  /*  Selected  foothold  in  body  coordinates  */ 

bd  footposj7::  ;*  Desired  foot  position  in  body  coordinates  *  / 

static  vector  d  footpos  7i:  /*  Desired  foot  position  in  earth  coordinates  */ 
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fool  _trajectory  *  / 


i  =  leg_number; 

if  (“selected  gait  -  =  FTL_GA1T) 

{ 

if  (transferflagiij  !=  ON) 

{ 

transfer_flag;ij  =  ON; 
liftoff_flagjij  =  OFF; 
place  flagjij  =  OFF; 

/*  Save  foothold  position.  * / 
oldfhjij.x  =  fh ;  i) -x; 
oldfhjij.y  =  fhjij.y; 
oldfhjij.z  =  fh|i|.z; 

proj  dist  =  LENGTH  *  0.21666; 

switch  (i) 

{ 

case  1:  '*  find  new  left  foothold  */ 

vx=  trans  rate->x; 

vy  =  trans  rate- >y  *  rot_rate->z  *  HALFLENGTH: 
rel  hd  =  atan2(vy,vx): 
bfhjL.y  =  82.0; 

bfh  1  x  =  HALFLENGTH-t-proj  dist*cos(rel  hd)  (82.0- 
proj  dist*sin(rel  hd))*tan(re)  hd); 
bfh ;  1  Lz  = -160.0; 

/*  Transform  to  earth  coordinates.  */ 

/*  fh  .  T  =  h  *  jbfh  ij)T  */ 

transform  point(fh,h,bfh,l); 
break; 

case  2.  /*  find  new  right  foothold  */ 

vx—  trans_rate->x; 

vy  =  trans_rate->y  -+-  rot_rate->z  *  HALFLENGTH; 
rel  hd  =  atan2(vy,vx); 
bfh[2].y  =  -82.0; 

bfhi2|.x  =  HALFLENGTH-t-proj_dist*cos(rel_hd)-(-82.0- 
proj_dist*sin(rel_hd))*tan(rel  hd); 
bfh  [2] .  z  =  -160.0; 

/*  Transform  to  earth  coordinates.  */ 

/*  |fh|il)T  =  h  *  [bfh.ijJT  •/ 

transform  _point(fh,h,bfh,2); 
break; 

default:  /*  back  leg  uses  old  front  leg  foothold  *  / 
fhiii.x  =  oldfh[i-2|.x; 
fh  i;.y  =  oldfh[i-2j.y; 
fh  i  .z  =  oldfhii-2  .z; 

} 
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*  foot_trajectory  *  ' 

/*  determine  the  desired  foot  position  */ 
dfootposjij.x  =  fh|i].x; 
d  footposji  y  -  fh|i|.y; 
d  footposii  z  =  fhji  .z; 

} 

} 

else  /*  FWD_WAVE_GAIT  Calculate  anew  desired  foot  position 
at  each  time  increment.  */ 

{ 

/*  Calculate  the  desired  touchdown  point.  */ 

!*  Future  change  note:  Change  from  cwv  center  to  midstance.  * j 

/*  Calculate  foot  velocity  at  center  of  cwv  (body  coordinates)  */ 
cwv_velocity.x  =  trans  rate->x  -  rot_rate->z  *  cwvjij.y. center 
■+•  rot_rate->y  *  cwvjij.z. center; 

cwv  velocity.y  =  trans  rate- >y  -  rot_rate->z  *  cwv  i  .x. center 
-  rot  rate->x  *  cwvji  z. center: 

cwv  velocity. z  =  trans_rate-.>z  -  rot  rate->y  *  cwvjj.x. center 
rot  rate- >x  *  cwvjij.y. center: 

/*  Calculate  the  time  to  reach  the  limits  of  the  cwv.  *  ■ 
if  (cwv  velocity. x  <  0.0) 

{ 

time  to  limit. x  =  (  cwv  ij.x.min  -  cwv  i  ,x.center)/cwv  velocity. x; 

) 

else  if  (cwv  velocity. x  >  0.0) 

{ 

time  to  limit. x  =  (  cwv  ij.x.max  -  cwvji], x. center) ,'cwv  velocity  x; 

> 

else 

{ 

time  to  limit.x  =  LONG  TIME; 

} 

if  (cwv  velocity. y  <  0.0) 

{ 

time  to  limit.y  =  (  cwvii].y.min  -  cwv  i  .y  . center) 'cwv  velocity. y: 

> 

else  if  (cwv  velocity. y  >  0.0) 

{ 

time  to  limit.y  =  (  cwv  ij.y.max  -  cwv  i  v  center)  cwv  velocity  v; 

} 

else 

time  to  limit. y  =  LONG  TIME; 
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tu  1  A/t.  .1  ■ 


^4  1.4  >■>  *.i 


foot  trajectory  * 

if  (cwv  velocity. z  <  0.0) 

{ 

time  to  limit. z  =  (  cwv|ij.z.min  -  cwv  i  z. center)  cwv  velocity. z; 

} 

else  if  (cwv_velocity.z  >  0.0) 

{ 

time  to  limit. z  =  (  cwv|i).z. max  -  cwv|ij.z.center)/cwv  velocity. z; 

> 

else 

< 

time  to  limit. z  =  LONG  TIME: 

} 

/  *  Determine  the  minimum  time  to  reach  the  cwv  limit.  */ 
min  time  =  time  to  limit. x; 
if  (time  tolimit.y  <  mintime) 

{ 

min  time  =  time  to  limit. y: 

} 

if  (time  to  limit. z  <  min  time) 

{ 

min  time  =  time  to  limit. z; 


'*  Calculate  the  desired  touchdown  point  in  body  coordinates.  */ 

/  *  Note:  This  point  changes  if  the  body  is  in  motion.  */ 

bd  footpos  i  x  -  cwvjij.x. center  +  cwv  velocity. x  *  min  time  *  9; 

bd  footpos  i  y  -  cwv|ij.y  center  +  cwv  velocity. y  *  min  time  *  9; 

bd  footpos  i  ,z  -  cwvii  ,z. center  -  cwv  velocity. z  *  min  time  *  .9; 

/*  Transform  to  Earth  coodinates.  * ! 

/*  id  footpos  i  T  =  h  *  | bd  footpos) ij IT  */ 
transform  point(d  footpos, h.bd  footpos, i); 


/*  Calculate  the  time  remaining  in  the  transfer  forward  phase.  *  1 
trans  fwd  time  =  “transtime  *  (‘beginplaee  phase  -  ‘trans  phase); 

<*  Calculate  the  new  foot  position.  (Earth  Coordinates)  */ 
if  (DELTA  TIME  <  transfwdtime) 

{ 

footpos  i.,x  -r  =  (d  footpos  i  ,x  -  footpos  i  .x) 

*  DELTA  TIME  /  trans  fwd  time; 
footpos  i  >  +=  (d  footpos  i  .y  -  footpos  i  .y) 

*  DELTA  TIME  /  trans  fwd  time; 
footpos  ii.z  -  footpos[i|.z;  *  Level  ground  assumption!  * 


Vi 


i  *  foot  trajectory  * 

else  *  Last  increment  of  time  * 

{ 

footposjr.x  =  d  footpos|ij.x; 
footposli  v  =  d  footposji|.y: 

footpos|i!.z  -  footposiz;  /*  Level  ground  assumption!  */ 

} 

/*  Transform  to  body  coodinates.  */ 

/*  |b  footposji|lT  =  invh  *  [footposjil  T  */ 
transform _point(b  foot pos, invh, footpos.i); 

}  /*  end  of  transfertrajectory  */ 


*  foot  trajectory  *,• 


placemen)  trajectory(liftoff  flag. place  flag, transfer  flag.footpos. 
b  footpos.invh.trans  time.trans  phase.  leg  number) 

/*  This  function  calculates  the  trajectory  of  the  foot  while  it  is 

being  lowered  from  the  ground.  It  is  called  from  foot  trajectory ().*/ 

vector  footpos[7),  /*  Present  foot  position  in  earth  coordinates  */' 
b  footpos|7|;  /*  Present  foot  position  in  body  coordinates  */ 

float  *trans_time, 

*trans_phase, 

in vh [4  [4  :  /*  Inverse  homogeneous  transformation  matrix  */ 

int  liftoff  flag|7l,  /*  Indicates  the  first  time  entering  subroutine 
in  current  leg  cycle  */ 
transfer  flag:7j, 
place  flag!7  , 
leg  number: 

{ 

float  placetime: 
in)  i; 

static  vector  d  footpos[7j;  /*  Desired  foot  position  in  earth  coordinates  * j 
i  -  leg  number: 

/*  Calculate  the  desired  foot  position.  */ 
if  (  place_flag(il  !=  ON) 

{ 

d  footposjij.z  =  footposjij.z  -  FOOTLIFTHEIGHT; 
liftoff  flagi  i  |  =  OFF; 
transfer  flag! ij  =  OFF; 
place  flag  ii  =  ON: 

} 

/*  Calculate  the  time  required  to  reach  the  desired  height 
from  the  present  foot  position.  */ 
place  time  =  *trans_time  *  (1.0-  *trans_phase); 

/*  Calculate  the  new  foot  position.  (Earth  Coordinates)  */ 
if  (DELTA  TIME  <  place_time) 

{ 

footposji  .z  +=  (d_footposii  .z  -  footposji  ,z) 

*  DELTA  TIME  /  place  time; 

} 
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foot  trajectory  * / 

else  *  Last  increment  of  time  */ 

{ 

footpos  i  .z  =  d_footpos[i].z; 

} 

/*  Transform  to  body  coodinates.  */ 

/*  [bfootposjijiT  =  invh  *  (footposji|jT  * j 
transform  _point(b_footpos,invh,footpos,i); 


}  /*  end  of  placement_trajectory 


*  / 

/ 


/*  foot  trajectory  *t 


/ 


support  trajectory (liftoff  flag.place  flag, transfer_flag,footpos, 
b  foot pos.invh.leg  number) 

/*  This  function  calculates  the  trajectory  of  the  foot  during  the 
foot  support  phase.  It  is  called  from  foot_trajectory().*/ 

vector  footposTj,  /*  Present  foot  position  in  earth  coordinates  */ 
b  footpos|7j;  /*  Present  foot  position  in  body  coordinates  */ 

float  invh[4j[4j;  Inverse  homogeneous  transformation  matrix  */ 

int  liftoff  flag(7'j,  /*  Indicates  the  first  time  entering  subroutine 
in  current  leg  cycle  */ 
transfer_flagi7j, 
place  flag[7i, 
leg  number; 


int  i: 

j*  In  this  phase  the  foot  is  kept  stationary  on  the  ground.  It 
is  assumed  that  the  foot  will  not  slip  or  move  accidently.  * / 

i  =  leg  number; 

/*  Transform  foot  position  to  body  coodinates.  *  j 
j*  b  footposj  T  —  invh  *  'footpos[i|jT  */ 
transform  _point(b_footpos,invh,footpos,i); 

/*  Turn  ofT  flags.  */ 
liftoffflagii  =  OFF; 
transfer  flaglij  =  OFF; 
placeflagjij  =  OFF; 


}  /*  end  of  support  trajectory  */ 


'**4****xX******xxx*****x*4******x*******vx****vx*x4x*x*****«x*****x 

This  function  is  for  the  program  walk.c  on  the  iris-2400, 
conwalk.c 


Based  in  part  on  J.H. Kessler’s 
R.  L.  Lyman  program  "conwalker.c" 

24  Apr  1987 

*****»*******«*****************s*t*****»**»*******»**************** 

^include  "gl.h" 

#include  "device. h" 

#include  "walk.h" 

/*  This  function  calls  up  the  walker  from  constructwalker  (with  legs 
already  properly  positioned)  and  then  rotates  and  translates  it  as 
commanded.  */ 

/*  Note:  Due  to  the  limited  number  of  bit  planes  available 
four  separate  walkers  are  constructed,  one  for  each  viewing 
quadrant.  The  walker  for  each  quadrant  is  drawn  from  furthest 
component  to  nearest.  This  provides  a  quasi-  Z  buffer  effect 
while  in  double  buffer  mode.  * / 

makewalker(machineobject,dl,d2, theta. knee, gamma, alpha. transrot  tag, 
tr  end  tag, walker, leg. thighobj,actuatorobj,shinobj, 
legmovetag,thighmovetag.actmovetag,shinmovetag  ,tx,ty,tz, 
roll, elev, azimuth, hx,hy,hz, 14) 

Tag  transrot  tag;4  ,tr  end  tag|4j,legmovetagj]j4j, 

thighmovetagj]i2:|4j,actmovetag[)[2i[4|,shinmovetag[|j2j|4]; 

Object  machineobjectl4!,walker[4],thighobj(][2j[4j,actuatorobjij[2j|4), 
shinobjj][2)  4j,legj][4j; 

int  dl[;,d2[j,kneefj(2|  ; 

Angle  thetajj,alphaj;,gamma||; 

float  tx,ty,tz, roll, azimuth, elev, 
hx  7 J ,hy (7j,hz(7] ,14 [7]; 

{ 

int  n; 
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'*  conwalk.c 

constructwalker(walker,dl,d2,  knee,  alpha,  gamma.thet  a.  leg,  thighobj, 
actuatorobj.shinobj.legmovetag.thighmovetag.actmovetag. 
shinmovetag.hx.hy,hz,l4)  ; 

for  (n=0;  n<4:  n+-t-)  /*  Rotate  and  translate  the  walkers  in  each 

quadrant.  */ 

{ 

machineobject|n]=genobj{); 
makeobj(machineobjectjnj); 
pushmatrix()  ; 

/*  Note:  Each  walker  is  built  on  the  origin.  Rotations  are  done 
before  translating  to  the  proper  location.  */ 
transrot  tagjn!=gentag(); 
m  aket  ag  ( tr  an  srot  _t  ag  j  n ! ) ; 

translate!  tx,ty,tz); 
rotated  in  t)  (elev  *  573), ’Y’); 
rotate(fint)  (roll  *  573). ’X’); 
rotate((int)  (azimuth  *  573), ’Z’); 

tr_end_tagjn!=gentag(); 
maketag(tr  end  tagjn  ); 

ft 

callobj(walker[nj); 
popmatrix()  ; 

closeobjO;  * 

}  ,  *  end  quadrant  loop  */ 

}  *  end  of  makewalker  * / 


/*  conwalk.c  * 


makeground(groundobject) 

lm  This  function  creates  a  checkerboard  groundplane  below  the  ASV  object.*  ' 
Object  ‘groundobject; 

{ 

Object  squareobject; 

Tag  transqrtag; 

static  int 

groundl|4j(3  ={{  1000.-500,0},  {1000, 500.0},  {-1000, 500,0},  {-1000, -500,0}}, 
ground2(4]  js;  =  {{  2000, -1000, 0},{2000, 1000, 0},{-2000, 1000,0},  {-2000,  -1000,0}}, 
square|4i|3]  =  {{0,- 100,0},  {0,0,0},  {-100, 0.0},  {-100, -100,0}}; 

int  ij: 

float  tx,ty; 

squareobject  -genobj(): 
makeobj  (squareobject); 
color(  WHITE); 
polfi  (4,  square); 
closeobj(): 

*groundobject=genobj(); 
makeobj  (*groundobject); 

color(RED);  /*  fill  outer  background  squares  */ 
polfi(4,ground2); 

color(GREEN);  /’*  fill  inner  background  squares  */ 
polfi(4,groundl); 

for  (i=0;  i<40;  i+  +  ) 

{ 

for  (j=0;  j<20;  j-+-  +  ) 

{ 

if  ((i+j)%2  <  1) 

{ 

tx=(i-20)*(- 100.0); 
ty=(j-10)*(-100.0); 
pushmatrixf); 
translate(tx,ty,0.0); 

callobj(squtreobject),  /*  place  the  white  squares  */ 
popmatrix(); 

}  /*  end  if*/ 

}  /*  end  for  j  */ 

}  /*  end  for  i  */ 
closeobj(); 

}  /*  end  makeground  */ 


/  *  conwalk.c  * 


/ 


constructwalker(  walker, dl,d2, knee, alpha, gamma, theta,  leg.  thighobj, 
actuatorobj,shinobj,legmovetag,thighmovetag.actmovetag, 
shinmovetag,hx,hy,hz,l4) 


/*  This  is  where  the  walker  is  made.  Here  each  part  is  assembled 
and  then  the  parts  are  put  together.  This  assembled  walker  is 
then  rotated  and  translated  in  makewalker  which  is  called  by 
the  main  program.  */ 


Tag  legmovetag(][4!  ,thighmovetag[|j2j[4j,actmovetag[j[2j[4’, 
shinmovetagj]j2][4|; 

Object  walker4  .leg  [4  ,thighobj[ ] [2 J [4 j,actuatorobj[ ! [2 j |4],shinobj } J [2] [4  ; 
int  dl,,d2.kneej|2l; 


Angle  alpha), gamma[j,thetajj; 

float  hx;7],hy|7),hz[7j,  j*  leg  pivot  position  * ! 
14[7|; 


{ 

Object  body, head. eye. boxobj[7j  ; 

static  float  legx  7|  =  {0.0.155  0.155.0,0.0,0.0,-155.0,-155.0}, 

legy  7  ={0.0,82.0,-82.0,82.0,-82.0,82.0,-82.0},  >» 

legz  j  7  =  {0.0.0.0,0.0,0.0,0.0,00,0.0}; 

Coord  x,y,z  ; 

int  ij,k,n,legnum  ; 

static  int 

/*  Coordinates  for  building  the  body  of  the  asv  */ 
blackbody[4j[3j  =  {{206,50,22},{206,-50,22},{206.-30,-101},{206,30,-101} }, 
lbodyarry[4|  (3|  =  {{-200, 30, -101},  {-200,50,22},  {206,50, 22},{206, SO, -101}}, 
rbodyarry|4][3|  =  {{-200, -30,  -101},  {206, -30, -101},  {206.-50.22},  {-200, -50, 22}}, 
tbodyarry;4]|3j  =  { {-200,50  °2}, {-200, -50, 22}, {206, -50, 22}, {206, 50, 22}}, 
bbodyarry|4!jSj  =  {{206, -30, -101}, {-200, -30, -101  },{-200,30,-10l}, {206.30,-101}}, 
backbodyarry|4  !S!={{-200,30,-101},{-200,-30,-101 },{ -200. -50, 22}, {-200, 50, 22}}, 
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/*  conwalk.c  */ 


/*  Coordinates  for  building  the  hydraulics  housing  structure  */ 

front  rt  top  4 j [3 j  =  { {27,-25. 10} , { 38 ,-25,- 13} ,{38,- IS,- 1 3 } , {27,- 1 3, 16} } , 

front  _rt_bttm|4]|3j  =  {  {38,-25.-13},  {38, -25, -46},  {38, -13.-46},  {38,  -13,  -13}}, 

rt  interior  5jj3]  =  { {20, -25, 38}, {38, -25, -13}, {38, -25.-46}, {-38.-25.-46}, {-38, -25, 38}}, 

rt  side|5  |S]= { {-38,-25.38} , {-38,-25,-46>,{ 38,-25,-46) ,{ 38.-25.- IS }.{ 20,-25,38 }} , 

It  interior(5|  3!  =  {{-38,25,38},{-38,25,-46},{S8,25,-46},{38,25,-13},{20,25,38}}, 

It  side|5j[3j  =  {{20,25,38},{38,25,-13},{38,25,-46},{-38,25,-46},{-38.25.38}}, 
top  box!4][3  ={{20, -25, 38}, {20, 25, 38}, {-38, 25.58}, {-88, -25,38,}}, 
back  box:4l|S  ={{-38, 25, -46}, {-38, -25, -46}, {-38, -25.38}, {-38, 25, 38}}, 
front  top|4i;3;={{20, 25, 38}, {20, -25, 38}, {27,-25, 13}, {27.25.13}}, 
front  _lt_top|4  j[3]  =  {  {27,13,16},{38,13, -IS},  {38, 25,-13  },{27, 25,16} }, 
front  Jt_bttm|4)i3|={  {38, 13.-13},  {38, 13, -46},  {38, 25,-46},  {38, 25,-13} }, 
bttm  lt|4](Sj  =  {{38,25,-46},{38,13,-46},{-38,13,-46},{-38,25,-46}}, 
bttm  rt|4li'3]={{S8, -25, -46}, {38, -13,-46}, {-38, -13.-46}, {-38.-25, -46}}, 

highbox  _topj4]|3]  =  {{-8,-25,88},{8,-25,88},{8,25,88},{-8.25.88}}, 
highbox_front|4][3]  =  {{8,25,88},{8,-25,88},{  IO.-25. 38 },{ 10,25,38} }. 
highbox  back(4|;3l={{-8,-25,88},{-8,25,88},{-10  25, 38}, {-10, -25, 38}}, 
highbox  rt(4||Sl={{8,-25,88},{-8,-25.88},{-10,-25,38},{10,-25,38}}, 
highbox  lt[4  jS  ={ {-8,25,88}, {8, 25.88}, {10, 25, 38}, {-10,25, 38}}, 


rt  spar  front|4  [3  ={{79.-13.-20}. {79.-25, -20}, {79.-25, -30}, {79,-13,-30}}, 
rt  spar  top  4  3> {{79, -13,-20}, {38.-13.-19}, {38,-25,-19}, {79, -25, -20}}, 
rt  spar  bttm'4  [3  =  {{38.-13,-32}. {79.-13.-30}, {79.-25, -30}, {38,-25.-32}}, 
rt  spar  rt  4  3  ={{38, -25, -32}. {79, -25.-30}, {79.-25.-20}, {38, -25, -19}}. 
rt  spar  It  4  3  ={{79, -13, -30}, {38.-13.-32}, {38.-13, -19}, {79, -IS, -20}}, 


It  spar  front!4][3]  =  {{79,25,-20},{79.13,-20},{79.13,-30},{79,25,-30} }, 

It  spar  top;4||3|  =  {{79,25,-20},{38,25,-19},{38.13,-19}.{79,13,-20}}, 

It  spar  bttm[4||3j={{38.25,-32},{79,25,-30},{79,13,  30}, {38, 13, -32}}. 

It  spar  rt;4][3j  =  {{38,13,-32},{79,13,-30},{79,13,-20},{38,13,-19}}, 

It  spar  lt[4j!3]={{79,25,-30},{38,25,-32},{S8,25,-19},{79,25,-20}}, 

/*  cab  construction  arrays  */ 

cab  bottom[4][3  ={{305, -30, -101}, {206, -30, -101}, {206, 30, -101}, {305, 30, -101}}, 
cab_top;4i[3  ={{250.33, 74}, {206, S3, 74}, {206, -33, 74}, {250, -33, 74}}, 

cab_fwd_support;4)|3  ={{305,30,-101  },{305,41,-16},{305,-41,-16},{305,-30,-101 }}, 
cab  fwdJower|4],3)  =  { {305, 41, -16}, {318, 48, 8}, {318, -48.8}, {305.-41.-16}}, 
cab  fwd  upper(4||3|={{318,48,8}.{302,83,68},{302,-33,68},{318.-48,8}}, 
cab  fwd  ovhd|4)|3;  =  {{275.33, 68}, {250.33,74}, {250, -33, 74}. {275, -33, 68}}, 

cab  rt  support[4;|3]={{305.-30, -101}, {305,-41. -16}, {206, -41. -16}, {206,-30, -101}}, 
cab  rt  lower|4j;s  ={{305.-41. -16}, {318.-48, 8}, {206.-48. 8}, {206.-41.-16}}, 
cab  rt  upperj4j!3]  =  { {318, -48,8 },{ 302, -33,68 }.{ 206, -33,68 },{ 206.-48.8} }. 
cab  rt  ovhd|4)!3]  =  {{275, -33, 68}, {250, -33, 74}, {206.-33, 74}, {206.-33.68}}, 


i*  conwalk  c  * 

cab  It  support [4^  3  { {206, 30, -101},{206, 41, -16}, {305, 41, -16}, {305.30,-101}}, 

cab  jt  Joweri4;|S;--{  {206.41,  -16},  {206.48, 8},  {318, 48.8},  {305, 41,-16}}, 
cab  It  upper  4  3  ={{206.48, 8}, {206, 33,68}, {302, 33,68}, {818.48.8}}, 
cab  Jt  _ovhd!4j!3j={  {206,33,68},{206,3S,74},{250,33,74},{275,33,68} }, 

cab  aft  supporti4j[3l  =  {{206,-30,-10l},{206,-41,-16},{206,41,-16},{206,30,-101}}, 
cab  aft  lowerj4jj3;  =  {{20e,-41,-16},{206,-48,8},{206,48,8},{206,41,-16}}, 
cab  aft  upperj4||3j  =  {{206,-48,8},{206r33,68},{206,33,68},{206,48,8}}, 
cab  aft_ovhd[4]|3]  =  {{206,-33,68},{206,-33,74},{206,33,74},{206,3S,68}}, 

scanner  fwd  lowerj4]l3]  =  {{302,S3,68},{322,33,95},{322,-33,95},{302,-S3,68}}, 
scanner  fwd  upper|4]|S|  =  {{322, S3, 95}, {322, 33,101}, {322, -33,101}, {322, -33, 95}}, 
scanner  rt[5)(3]  =  { {302, -S3, 68}, {322, -S3, 95}, {322, -33,101  },{275, -33,101  },{275, -33, 68}}, 
scanner  It [5||3|  =  { {302, 33, 68},{275, 33, 68}, {275,33, 101 },{ 322, 33, 101}, {322, 33,95} }, 
scanner  aft [4i [3j  =  {{ 275.33, 101 },{ 275,33,68 },{ 275,-33,68 >,{ 275,-33, 10 1 }} , 
scanner  top[4]|3l  =  {{322,33, 101}, {275, 35, 101}, {275, -S3, 101}, {322, -33, 101}}; 

/*  The  making  of  the  leg  is  quite  complicated.  Each  leg  consists  of  an 
upper  link  (thigh),  lower  link  (actuator),  and  a  shank  (shin).  These 
segments  are  first  defined  in  a  standardized  orientation,  and  are  then 
rotated  and  translated  into  the  proper  position.  This  is  done  by  using 
2  objects  for  each  segment.  The  first  object  is  the  correctly  rotated 
segment,  and  the  second  object  is  the  correctly  translated  first 
object.  Thus  the  segment  is  then  in  the  proper  position.  To  hold  the 
screen  coordinate  system  fixed  the  matrix  is  pushed  before  each  translation 
or  rotation  and  then  popped  after  the  object  is  constructed  or  called.  */ 

for  (n=0;  n<4;  n+  +  )  /*  Make  a  set  of  legs  for  each  viewing  quadrant 
Each  quadrant  must  have  unique  tags.  */ 

{ 

for(legnum=l  ;legnum<7;legnum++) 

{ 

/*  Each  segment  is  constructed  and  positioned  *  / 
buildthighfn, legnum, dl, alpha, thighobj.thighmovetag)  ; 
build  act  uator(n,  legnum,d2,  alpha,  actuatorobj,actmovetag)  ; 
buildsh in(n,legnum, knee, gamma, shinobj.shinmovetag)  ; 

leg[legnumjjnj=genobj(); 

makeobj(leg[legnum][n|); 

pushmatrixQ; 

/*  translate(legx[legnum|,legy  legnum  Jegz  legnum  )  ; 
translate(hx(legnum;,hy|legnum}.hz  legnumj)  ; 

legmovetag;iegnum!ini=gentag();  /*  The  leg  is  assembled  from  */ 
maketag(legmovetag[legnumJJn|);  j*  its  parts  and  the  entire  leg  is  */ 

/*  then  rotated  to  the  proper  angle.  */ 
rotate(theta. legnum  ,’X’). 

translate(0.0,14(legnum;,0.0);  /*  extend  leg  outward  * 
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conwalk.c  */ 


if  ( ( (n  >  l)&&(legnum  <  5) )| 

((n  <  2)&&(legnum  >  4)))  /*  Build  the  left  side  first.  * ! 

{ 

if  (legnum  >4)  /*  Reverse  the  back  legs.  */ 

{ 

pushmatrixf): 
rotatef  1800, ’Z’); 

} 

color(BLACK); 
polfi(5, It  interior); 

color(GREEN). 
polfi(5,lt_side); 
polfi(4,front_lt_top); 
polfi(4,front_lt_bttm); 
polfi(4,bttm  It); 

polfi(4,lt_spar_front); 
polfi(4,lt  spar  bum); 
polfi(4,lt_spar  It); 
polfi(4,lt_spar  rt); 

color(BLUE); 
polfi(4,lt_spar  top): 

color(BLACK); 
poly i( 4, It  spar  rt): 

color(CYAN)  ; 

callobj(thighobj|legnum  lj  n] ) ; 
callobj(actuatorobj  legnum ;| l)jn;); 
callobj(shinobjilegnumj!l]in]); 

color(GREEN)  ; 
polfi(4,rt  spar  front); 
polfi(4,rt_spar_bttm); 
polfi(4,rt_spar_lt); 
polfi(4,rt_spar_rt); 

color(BLUE); 
polfi(4,rt  spar  top); 

color(GREEN); 
polfi(4,front_rt  bttm); 
polfi(4, front  rt  top); 
polfi(4, front  top); 
polfi(4,bttm_rt); 
polfi(4,back  box); 
polfi(4,top_box); 
polfi(5,rt_side); 
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conwalk.c  / 


color(BLACK); 
polyi(4.top  box); 
polyi(5.rt  side); 
polyi(4.rt  sparrt); 

color(GREEN); 
polfi(4,highbox_front); 
polfi(4,highbox_lt); 
polfi(4,highbox_back); 
polfi  ( 4 ,  h  igh  box  _rt ) ; 
polfi(4,highbox_top); 

color(BLACK); 
polyi(4.highbox  top); 
poIyi(4.highbox  rt); 
polyi(4.highbox  back); 

if  (legnum  >  4)  /*  For  reversing  the  back  legs.  ‘ 

{ 

poprnatrix()  ; 

} 


} 

else  *  Build  the  right  side  first.  */ 

{ 

if  (legnum  >  4)  /*  Reverse  the  back  legs. 

{ 

pushmatrixf); 
rotate)  1800. ’Z’): 

} 

color(BLACK); 
polfi (5, rt  interior); 

color(GREEN); 
polfi(5,rt_side); 
polfi(4, front  rt  top); 
polfi(4, front  rtbttm); 
polfi(4,bttm  rt); 

polfi(4,rt  spar  bttm); 
polfi(4,rt  spar  rt); 
polfi(4,rt  sparjt); 
polfi(4.rt  spar_front); 

color(BLUE); 
polfi(4,rt  spar  top); 


/  *  conwalk.c  * 


color(BLACK); 

polyi(4,rt  spar  It); 

color(CYAN)  ; 

callobj(thighobj!legnumj|l|inj); 

callobj(actuatorobjjlegnuni):lj[nj); 

callobj(shinobj[legnum|[l]|n]); 

color(GREEN)  ; 
polfi(4, back  box); 
polfi(4,bttm_lt); 
polfi(4, front  top); 
polfi(4,  front  _lt  _bttm); 
polfi(4, front _lt  top); 

polfi(4,lt  spar  bttm); 
polfi(4,It_spar  rt); 
poIfi(4,lt_spar  It); 
polfi(4,lt_spar  front); 

color(BLUE); 

polfi(4.lt  spar  top); 

color(GREEN): 
polfi(4.top  box); 
polfi(5.1t  side); 

color(BLACK); 
polyi(4,top  box); 
polyi(5,lt_side); 
polyi(4,lt_spar_lt); 

color(GREEN); 
polfi(4,highbox  back); 
polfi(4,highbox  jrt); 
polfi(4.high box  front): 
polfi(4,highbox  It); 
polfi(4,highbox  top); 

color(BLACK); 
poly i(4,highbox  top); 
poly  i(4, high  box  Jt); 
polyi(4,highbox_front); 
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,  *  con  walk. c  *  1 

if  (legnum  >  4)  For  reversing  the  back  legs.  *, 

{ 

popmatrix))  ; 

} 

} 

popmatrix(): 
closeobjd  : 

}  j*  end  of  leg  loop  */ 

}  /*  end  of  quadrant  loop  * / 

body=genobj()  :  /*  The  body  is  constructed  */ 

makeobj(body); 

color(LT YELLOW)  ; 
polfi(4,lbodyarry)  ; 
polfi(4,backbodyarry)  ; 
polfi(4,bbodyarry)  : 
polfi(4,rbodyarry)  ; 

color)  YELLOW): 
polfi(4.tbodvarry); 

color(BLACK)  ; 
polfi(4,blackbody)  : 
closeobj()  ; 

head  =  genobj()  ;  /*  construct  the  head  */ 

makeobj(head)  : 
color(YELLOW); 
polfi(4,cab  top); 
polfi(4,cab_fwd_ovhd); 
polfi(4,cab  rt  ovhd); 
polfi (4,cab _lt  ovhd); 
polfi(4,cab  aft  ovhd); 

color(BLACK); 
polfi  ( 4  ,cab  bottom ) ; 
polfi(4.cab  fwd  support); 
polfi(4.cab  rt  support); 
polfi(4,cab  It  support); 
polfi ( 4, cab  aft  support ) ; 

color)  WHITEl); 
polfi(4,cab  fwdjower); 
polfi(4,cab  rt  lower); 
polfi(4.cab  It  lower); 
polfi(4,cab_aft_lower): 


162  3 


’•f»v«y»:r‘V*Yw 


I*  conwalk.c  */ 


color(WHITE)  ; 
polfi(4,cab_fwd  upper); 
polfi(4,cab_rt_upper); 
polfi(4,cab_lt  upper); 
polfi(4,cab_aft  upper); 

color(BLACK)  ; 
polfi(4,cab_top); 
polyi(4,cab_fwd_lower); 
polyi(4,cab_fwd  upper); 
polyi(4,cab_fwd_ovhd); 
poly i(4,cab_rt  lower); 
polyi(4,cab  rt  upper); 
polyi(4,cab_rt  ovhd); 
polyi(4,cab_lt  lower); 
polyi(4,cab_lt  upper); 
polyi(4,cab_lt  ovhd); 
closeobj()  ; 

eye=genobj()  ;  /*  contruct  the  radar  (eye)*/ 

makeobj(eye)  ; 
rolor(RED); 

polfi(4, scanner  fwd  upper); 
polfi (4. scanner  fwd  jower); 
polfi(5, scanner  rt): 
polfi (5, scanner  It); 
polfi(4, scanner  aft); 

color(BLACK)  ; 
polfi(4,scanner_top); 

color(BLUE)  ; 
closeobj()  ; 

walker|Oj=genobj();  /*  assemble  all  the  parts  for  quad  I  */ 

makeobj(walker|0]);  /*  back  and  right  first  */ 

c&llobj ( leg ( 6 ] ( 0  ); 
callobj(leg|4)[0:); 
callobj (leg [ 2 j  (0  ) ; 
callobj(body); 
callobj  (head); 
callobj(eye); 
callobj(leg(5}[0  ); 
callobj( leg ( 3 < [0  ); 
callobj(legjl!|0  ); 
closeobj()  ; 


/*  conwalk.c  * / 


walker  j  1 J  =genobj() ; 
tnakeobj(walker[lj); 
callobj(leg|2][l)j; 
caUobj(leg!4][lj); 
callobj  (legjfijjlj); 
callobj  (head); 
callobj(eye); 
callobj(body); 
calk>bj(legjl]|ij): 
c«llobj(  leg[3]  [  1  j ); 
callobj  ( leg ;  5]  [  1 1 ) ; 

walker[2] = genobj  ( ) ; 
makeobj  (walker' 2  j ) ; 
callobj  ( leg  [  1  j  [  2) : 
calk>bj(leg(3][2|); 
callobj  (leg 1 5j  (  2  j ) ; 
callobj  (head); 
callobj  (eye); 
callobj(body); 
callobj(leg(2](2{); 
c  allobj  ( leg  1 4  j  j  2  j ) ; 
c  allobj  ( leg  j  ©j  j  2  j ) ; 

walker|3j  =  genobj(); 
makeobj  (walker!  3  ] ) ; 
callobj  (leg  5]  [3  j  j ; 
callobj(leg!3){3j); 
callobj  ( leg !  1  jj  3  j ) : 
callobj(body); 
callobj(head); 
callobj(eye); 
caUobj(leg[6j[3j); 
callobj(leg(4][3|); 
c  allobj  ( leg  j  2  j  j  3  j ) ; 

} 


/*  assemble  all  the  parts  for  quad  II  */ 
f*  front  and  right  first  */ 


/*  assemble  all  the  parts  for  quad  III*/ 
/*  front  and  left  first  */ 


/*  assemble  all  the  parts  for  quad  IV  */ 
!*  back  and  left  first  */ 
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/*  conwalk.e  *! 


/*************»*****»************************************»******»***** 
buildthigh(n,legnum,dl, alpha, thighobj  .ihighmovetag) 

/*  this  function  constructs  the  thigh  (upper  link)  and  rotates,  then 
translates  it  into  the  proper  position  */ 

Tag  thighmovetag[)|2j(4|; 
int  n, legnum.dlj)  ; 

Angle  alpha|]  ; 

Object  thighobj[]>2][4j; 

{ 

static  int 

thighltside[4j[3j={{0,10,7},{l02,10,7},{102,10,-7},{0,10,-7}}, 
thighrtside[4|(3j  =  {{0,-10,-7},{l02,-10,-7},{  102, - 10,7  },{0.- 10,7) }, 
thighfront[4j[3j={{0,-l0,7},{102,-10,7},{  102,10,7},{0,10,7}}  . 
thighbttmj4|  [3  j={  {0,10, -7},{  102, 10,-7 },{ I02,-10,-7},{0,-10,-7}}; 


thighobj  (legnumi|0jnJ=genobj(); 
makeobj(  thighobj  |  legnum]  [0]  [n] ) ; 

pushmatrix()  ; 

thighmovetagllegnumi!0|[n]=gentag();  /*  rotate  thigh  */ 
maketag(thighmovetag  legnum; jOjjnj); 
rotaie(aJphaj]egnumJ,’Y’)  ; 

if( legnum 4)  *  Build  the  left  side  first.  */ 

{ 

color(CYAN); 

po)fi(4.thighbttm); 

polfi(4,thighltside); 

polfi(4,thighrtside); 

color(RED); 

polfi(4.thighfront); 

color(BLACK); 

polyi(4,thighrtside); 

} 


r  con  walk,  c  */ 

else  /*  Build  the  right  side  first.  */ 

{ 

color(CYAN); 

polfi(4,thighbttm); 

polfi(4,thighrtside); 

polfi(4,thighltside); 

color(RED); 

polfi(4,thighfront); 

coIor(BLACK); 

polyi(4,thighltside); 

} 

popmatrixf)  ; 
closeobj()  ; 

thighobj[legnumj(l|lnj=genobj()  ; 
makeobj(thighobj'legnumjjlj[n|)  ; 

push  matrix))  ; 

thighmovetagilegnum|jlj;n]=gentag(); 

maketag(thighmovetag[legnum|(l{[n|);  /*  translate  thigh  */ 
translate(0. 0,0.0,  (float  )(-d  1  [  legnum  J ) )  ; 

eailobj(thighobj  legnum](0][n]); 
popmatrix))  ; 

closeobj()  ; 


/*  conw&lk.c  * / 


y******************************«*************************** ***********  ; 
buildactuator(n,legnum.d2,alpha.actuatorobj,actmovetag) 

/*  construct  the  actuator  (lower  link)  */ 

Tag  actmovetagjj[2](4|; 

int  n,legnum,d2jj; 

Angle  alpha)]; 

Object  actuatorobj|][2](4j; 

{ 

static  int  actltside|4] [3] ={  {0, 10,5} ,{83, 10,5} 83, 10,-5 >, {0, 10,-5} } , 
actfront [4](3i  =  {{0,-10,5},{83,-10,5},{ 83 . 10.5} ,{0, 10,5} }  , 
actrtside[4][3]={{0,-10,-5},{83,-10,-5},{83,-10,5},{0,-10,5}}. 
actbttm[4]jS]  =  {{0,10,-5},{83,10,-5},{83,-10,-5},{0,-10,-5}}  ; 


actuatorobjjlegnumj  0jjn;=genobj(); 
makeobj(actuatorobj  legnum][0]|n  ); 

pushmatrixf): 

actmovetag  legnumj  0|(n;=gentag(); 

maketag(actmovetag:legnum]jO][n));  /*  rotate  actuator  */ 
rotate(alphailegnumj,’Y’)  ; 

if(legnum>4)  /*  Build  the  left  side  first.  */ 

{ 

color(CYAN); 

polfi(4,actbttm); 

polfi(4,actltside); 

polfi(4,actrtside); 

color(RGD); 
polfi(4, actfront): 

color(BLACK); 

polyi(4,actrtside); 

} 
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else  /*  Build  the  right  side  first.  */ 

{ 

color(CYAN); 

polfi(4,actbttm); 

polft(4,actrtside); 

polfi(4.actltside); 

color(RED); 

polfi(4,actfront); 

color(BLACK); 

polyi(4,actltside); 

} 

popmatrixQ; 

closeobjQ; 

actuatorobj[legnum{[l][n  =genobj(); 
makeobj  ( ac  t  u  at  orobj  |legnum  j  1 1  i  {n  j ); 

pushmatrixQ: 

actmovetag[legnumj|lj|n  -=gentag(); 

maketag(actmovetag|legnum|[l|lnl);  /*  translate  actuator  */ 
translate!  (float  J  (d2  jlegnum  j),0.0,(float)(-L3) ) ; 
callobj(actuatorobj(legnum||0{{nJ); 
popmatrixQ; 
closeobj(); 

}  /*  end  of  buildactuator  */ 


/*  con  walk  ,  c  */ 


•  / 

buildshin(n,legnuin,  knee,  gamma,  shinobj.shinmovetag) 

/*  construct  the  shank  (shin  )  * / 

Tag  shinmovetagj|[2)!4  : 

•  int  n,legnum,knee[Ji2j; 

Angle  gamma)]; 

Object  shinobjl]  2’  14]; 

{ 

static  int 

shinltside{6]jSj={{6,5,3},{  10,5,-59},{-7,5,-50},{-6,5,3},{-3,5,6},{3,5,6}}, 
shankltside:4]i'3l={{10,5,-59},{-23,5,-102},{-36,5,-100},{-7,5)-50}}  , 
shinfront!4!|S={{6,5,3},{6,-5,3},{l0,-5,-59},{10,5,-59}}, 
shankfront|4:|3j:={{l0,-5,-59}.{-23,-5,-102},{-23,5,-102},{7,5,-59}}, 
ankleltside(6:j3|  =  {{-23.5,-l02},{3,5,-153},{2,5,-I57},{-3,5,-158},{-6,5.-158},{-36,5,-100}}, 
shinrt3ide'6)jSj={{3,-5,6},{-S,-5,6},{-6,-5.S},{-7,-5.-50},{10,-5,-60},{6,-5,3}}, 
shankrtside(4)[3]={{-7,-5,-50},{-36,-5,-100},{-23,-5,-102}.{10,-5.-59}}  , 
anklertside|6||3]  =  {{-36,-5,-100},{-6,-5,-158},{-3,-5.-158},{2,-5,-157}.{3,-5,-153}.{-23.-5.-102}}. 
anklefrontj4|(3]={{-23, 5.-102}, {-23, -5, -102}, {3, -5.-153}, {3.5, -153}}. 
shinbackl4jj3]={{-7,-5,-50},{-6,-5,3}.{-6,5,3}.{-7,5,-50}}, 
shankback[4j|3l  =  {{-36,-5,-100},{-7,-5,-50},{-7,5,-50},{-36,5,-100}}, 
ankleback|4]|3;  =  {{-6,-5,-158},{-36,-5,-100},{-S6.5,-100},{-6.5,-158}}, 
j  bottom  fwd|4][3]={{3,5,-153},{2,  5, -157}, {2, -5, -157}, {3, -5.-153} }, 

bottom  mid>4]!3;  =  {{2,5,-157},{-3,5,-158},{-3,-5,-158},{2,-5,-157}}, 
bottom  aft(4l[3|={(-3,5,-158},{-fl,5,-158},{-6,-5,-I58},{-3,-5,-158}}, 

<e>  shinobj|legnuml|Oj|n]=genobj(); 

makeobj(shinobj|legnumj|Oijn!); 
pushmatrix()  ; 

shinmovetagllegnum]|0j  nj=gentag(); 
maketag(shinmovetag|legnum][0]jn|);  /*  rotate  shank  *  j 
rotate(gammajlegnumj,’Y’); 

if(legnum>4)  j*  Build  the  left  side  first.  */ 

{ 

color(BLACK); 
polfi(4,bottom_fwd); 
polfi(4, bottom  mid); 
polfi (4, bottom  aft ); 

color(CYAN); 
polfi(4,ankleback); 
v  polfi(6,ankleltside); 

poifi(6,anklertside); 

color(RED); 

*  poifi(4,anklefront}  ; 
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color(CYAN); 
polfi(4.shankback)  ; 
polfi(4,shankltside); 
polfi(4,sh»nkrtside); 

color(RED); 
polfi(4.shankfront)  ; 

color(CYAN); 

polfi(4,shinback), 

polfi(6,shinltside); 

po!fi(6,shinrtside); 

color(RED); 
polfi(4.shinfront)  ; 

co)or(BLACK): 

polyi(6,anklertsid«); 

polyi(4,shankrtside); 

polyi(6.shinrtside); 

} 

else  t*  Build  the  right  side  first.  */ 

{ 

color(  BLACK); 
polfi(4.bottom_fwd)  ; 
polfi  (4,  bottom  _mid)  ; 
polfi(4,bottom_aft)  ; 

color(CYAN); 
polfi(4,ankleback); 
polfi  (6,  anklertside)  ; 
polfi(6,ankleltside); 

color(RED); 

po!fi(4.anklefront); 

color(CYAN); 
polfi(4,shankback)  ; 
polfi(4,shankrtside); 
polfi(4,shaokltsidej; 

color  (RED); 

polfi(4,shankrront); 

color(CYAN); 

polfi(4.shinback); 

polfi(6.shinrtside); 

polfi(6,shinltside); 
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/*  conwalk.c  */ 

color(RED); 
polfi(  4  ,sh  infront): 

“  color(BLACK); 

poly  i(6,ankleltside) ; 
polyi(4,shankltsid«); 

r  polyi(6,shinltside); 

} 

coIor(BLACK); 

pushmatrixQ; 
rotate(-900,’X’); 
translate(0.0,0.0,5.0); 
circf(0.0,0.0,7.0)  ; 
circffO.0, 32.0.5.0)  ; 
popmatrix(); 

pushmatrix(); 
rotate(900.’X’); 
translate(0.0,0.0,5.0); 
circf(0.0,0.0,7.0)  ; 
circf(0.0, -32.0, 5.0)  ; 
popmatrixQ; 

popmatrixf); 

closeobjf); 

shinobj'legnumj|l)[nj=genobj(); 
makeobj  (shinobj  [  legnum  |  [  1 )  [  n  | ) ; 

pushmatrix(); 

shin  movet  ag  ( legii  um  |  [  1  ]  [  n  | = gen  t  ag  ( ) ; 

maketag(shinmovetag|legnum][l][n]);  /*  translate  shank  */ 

translate((float)knee[legnum]|0],0.0,(float)knee|legnum][l])  ; 

callobj(shinobjjlegnum|[Oj[n)); 

popmatrixf); 

closeobjQ; 


}  /*  end  of  buildshin  */ 


171 


This  is  a  function  for  the  iris2400  program  walk.c. 
toolbox. c 


Relle  Lyman  25  Aug  1986 

*»****»*******S************»**»**»***««**********X*«******«**** ' 

/ 

#  include  "gl.h" 

#include  "device. h" 

^include  "walk.h" 

^include  <stdio.h> 

#  include  <math.h> 

/»***»*********************»*********»*********«***»************************* 
transform  point(p2,m,pl,i) 

/*  This  function  changes  the  coordinate  system  for  a  point  vector 

using  a  homogeneous  transformation  submatrix.  p2  =  m  *  pi  */ 

int  i;  /*  Leg  number  */ 

float  m;4 j [4 j ;  /*  Homogeneous  transformation  submatrix  */ 

vector  pl|7|,  /*  Vector  represented  in  first  coordinate  system  */ 

p2[7j;  /*  Vector  represented  in  transformed  coordinate  system  *  j 

{ 

p2ji].x  =  m|0||0!*pl(ij.x  -+•  m;0](lj*pl[i].y  +  m[0]|2]*pl|i;.z  m|0]|8j; 

p2ji].y  =  m|l][0]*pl|i].x  +  mT]|lj*pl(ij.y  +  mjlj|2]*plji;.z  +  m[  1  j  [3j; 
p2  ij.z  =  m’2|i0|*pl[il.x  +  mj2Hl|*pl'ij.y  -t-  mS2]l2j*pliij.z  + 

}  /  *  end  of  transform  point  */ 

/*****************************************»***********»»*****»********»****** 
float  modulus_one(temp) 

/*  This  function  performs  the  modulus  one  operation  on  numbers  of  type  float.  */ 

float  temp; 

{ 

while  (temp  >  =  1.0) 

{ 

temp  -=  1.0; 

} 

while  (temp  <  0.0) 

{ 

temp  +=  1.0; 

} 

return  temp: 

}  j*  end  of  modulus  one  */ 


/*  Makefile  *, 


#  This  is  Makefile.  It  is  used  in  the  utility  make  to  speed 

#  compilation  of  walk.c.  To  use  it,  just  type  "make''. 

CFLAGS  =  -Zf-Zg  -g 

SRCS  —  walk.c 
conwalk.c 
support,  c 
toolbox. c 
steering. c 
body  rates. c 
ft  traj.c 
opt  period.c 
leg  phase. c 
con  work  vol.c 
driver. c 
status. c 
decelerate. c 
init.c 

OBJS  =  walk.o 
conwalk.o 
support. o 
toolbox. o 
steering. o 
body  rates. o 
ft  traj.o 
opt  period. o 
legphase.o 
conwork  vol.o 
driver. o 
status.o 
decelerate. o 
init.o 

walk:  (OBJS) 

cc  -o  walk  (OBJS)  -Zg  -Zf 


(OBJS)  :  walk.h 
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